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/BUILD b/y2016/BUILD
index ee77f00..f3887b5 100644
--- a/y2016/BUILD
+++ b/y2016/BUILD
@@ -1,4 +1,5 @@
load("//frc971:downloader.bzl", "robot_downloader")
+load("//aos:config.bzl", "aos_config")
cc_library(
name = "constants",
@@ -34,16 +35,18 @@
"//aos/logging",
"//aos/time",
"//aos/util:log_interval",
- "//frc971/autonomous:auto_queue",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//frc971/autonomous:auto_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_goal_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//frc971/queues:gyro",
"//y2016/actors:autonomous_action_lib",
"//y2016/actors:superstructure_action_lib",
"//y2016/actors:vision_align_action_lib",
- "//y2016/control_loops/shooter:shooter_queue",
+ "//y2016/control_loops/shooter:shooter_goal_fbs",
+ "//y2016/control_loops/superstructure:superstructure_goal_fbs",
"//y2016/control_loops/superstructure:superstructure_lib",
- "//y2016/control_loops/superstructure:superstructure_queue",
- "//y2016/queues:ball_detector",
+ "//y2016/control_loops/superstructure:superstructure_status_fbs",
+ "//y2016/queues:ball_detector_fbs",
],
)
@@ -65,6 +68,28 @@
],
)
+aos_config(
+ name = "config",
+ src = "y2016.json",
+ flatbuffers = [
+ "//y2016/control_loops/shooter:shooter_goal_fbs",
+ "//y2016/control_loops/shooter:shooter_output_fbs",
+ "//y2016/control_loops/shooter:shooter_position_fbs",
+ "//y2016/control_loops/shooter:shooter_status_fbs",
+ "//y2016/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2016/control_loops/superstructure:superstructure_output_fbs",
+ "//y2016/control_loops/superstructure:superstructure_position_fbs",
+ "//y2016/control_loops/superstructure:superstructure_status_fbs",
+ "//y2016/queues:ball_detector_fbs",
+ "//y2017/vision:vision_fbs",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/robot_state:config",
+ "//frc971/control_loops/drivetrain:config",
+ ],
+)
+
cc_binary(
name = "wpilib_interface",
srcs = [
@@ -78,16 +103,16 @@
"//aos:math",
"//aos/controls:control_loop",
"//aos/logging",
- "//aos/logging:queue_logging",
- "//aos/robot_state",
+ "//aos/robot_state:robot_state_fbs",
"//aos/stl_mutex",
"//aos/time",
"//aos/util:log_interval",
"//aos/util:phased_loop",
"//aos/util:wrapping_counter",
- "//frc971/autonomous:auto_queue",
- "//frc971/control_loops:queues",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//frc971/autonomous:auto_fbs",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_output_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_position_fbs",
"//frc971/wpilib:ADIS16448",
"//frc971/wpilib:buffered_pcm",
"//frc971/wpilib:dma",
@@ -97,16 +122,18 @@
"//frc971/wpilib:gyro_sender",
"//frc971/wpilib:interrupt_edge_counting",
"//frc971/wpilib:joystick_sender",
- "//frc971/wpilib:logging_queue",
+ "//frc971/wpilib:logging_fbs",
"//frc971/wpilib:loop_output_handler",
"//frc971/wpilib:pdp_fetcher",
"//frc971/wpilib:sensor_reader",
"//frc971/wpilib:wpilib_robot_base",
"//third_party:wpilib",
"//y2016/control_loops/drivetrain:polydrivetrain_plants",
- "//y2016/control_loops/shooter:shooter_queue",
- "//y2016/control_loops/superstructure:superstructure_queue",
- "//y2016/queues:ball_detector",
+ "//y2016/control_loops/shooter:shooter_output_fbs",
+ "//y2016/control_loops/shooter:shooter_position_fbs",
+ "//y2016/control_loops/superstructure:superstructure_output_fbs",
+ "//y2016/control_loops/superstructure:superstructure_position_fbs",
+ "//y2016/queues:ball_detector_fbs",
],
)
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();
diff --git a/y2016/control_loops/drivetrain/BUILD b/y2016/control_loops/drivetrain/BUILD
index 629c957..e706f25 100644
--- a/y2016/control_loops/drivetrain/BUILD
+++ b/y2016/control_loops/drivetrain/BUILD
@@ -1,5 +1,3 @@
-load("//aos/build:queues.bzl", "queue_library")
-
genrule(
name = "genrule_drivetrain",
outs = [
diff --git a/y2016/control_loops/drivetrain/drivetrain_main.cc b/y2016/control_loops/drivetrain/drivetrain_main.cc
index 58ba7ce..b4fd9d8 100644
--- a/y2016/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2016/control_loops/drivetrain/drivetrain_main.cc
@@ -1,6 +1,6 @@
#include "aos/init.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "y2016/control_loops/drivetrain/drivetrain_base.h"
@@ -9,7 +9,10 @@
int main() {
::aos::InitNRT(true);
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
&event_loop, ::y2016::control_loops::drivetrain::GetDrivetrainConfig());
DrivetrainLoop drivetrain(
diff --git a/y2016/control_loops/shooter/BUILD b/y2016/control_loops/shooter/BUILD
index 8938a2b..794c4fc 100644
--- a/y2016/control_loops/shooter/BUILD
+++ b/y2016/control_loops/shooter/BUILD
@@ -1,16 +1,37 @@
package(default_visibility = ["//visibility:public"])
-load("//aos/build:queues.bzl", "queue_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
-queue_library(
- name = "shooter_queue",
+flatbuffer_cc_library(
+ name = "shooter_goal_fbs",
srcs = [
- "shooter.q",
+ "shooter_goal.fbs",
],
- deps = [
- "//aos/controls:control_loop_queues",
- "//frc971/control_loops:queues",
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "shooter_position_fbs",
+ srcs = [
+ "shooter_position.fbs",
],
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "shooter_output_fbs",
+ srcs = [
+ "shooter_output.fbs",
+ ],
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "shooter_status_fbs",
+ srcs = [
+ "shooter_status.fbs",
+ ],
+ gen_reflections = 1,
)
genrule(
@@ -52,8 +73,11 @@
"shooter.h",
],
deps = [
+ ":shooter_goal_fbs",
+ ":shooter_output_fbs",
":shooter_plants",
- ":shooter_queue",
+ ":shooter_position_fbs",
+ ":shooter_status_fbs",
"//aos/controls:control_loop",
],
)
@@ -63,10 +87,13 @@
srcs = [
"shooter_lib_test.cc",
],
+ data = ["//y2016:config.json"],
deps = [
+ ":shooter_goal_fbs",
":shooter_lib",
- ":shooter_queue",
- "//aos:queues",
+ ":shooter_output_fbs",
+ ":shooter_position_fbs",
+ ":shooter_status_fbs",
"//aos/controls:control_loop_test",
"//aos/testing:googletest",
"//frc971/control_loops:state_feedback_loop",
@@ -80,9 +107,12 @@
"shooter_main.cc",
],
deps = [
+ ":shooter_goal_fbs",
":shooter_lib",
- ":shooter_queue",
+ ":shooter_output_fbs",
+ ":shooter_position_fbs",
+ ":shooter_status_fbs",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
],
)
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
diff --git a/y2016/control_loops/shooter/shooter.h b/y2016/control_loops/shooter/shooter.h
index 9e6968f..07fee84 100644
--- a/y2016/control_loops/shooter/shooter.h
+++ b/y2016/control_loops/shooter/shooter.h
@@ -4,11 +4,14 @@
#include <memory>
#include "aos/controls/control_loop.h"
-#include "aos/events/event-loop.h"
+#include "aos/events/event_loop.h"
#include "aos/time/time.h"
#include "frc971/control_loops/state_feedback_loop.h"
-#include "y2016/control_loops/shooter/shooter.q.h"
+#include "y2016/control_loops/shooter/shooter_goal_generated.h"
#include "y2016/control_loops/shooter/shooter_integral_plant.h"
+#include "y2016/control_loops/shooter/shooter_output_generated.h"
+#include "y2016/control_loops/shooter/shooter_position_generated.h"
+#include "y2016/control_loops/shooter/shooter_status_generated.h"
namespace y2016 {
namespace control_loops {
@@ -28,7 +31,8 @@
void set_position(double current_position);
// Populates the status structure.
- void SetStatus(ShooterSideStatus *status);
+ flatbuffers::Offset<ShooterSideStatus> SetStatus(
+ flatbuffers::FlatBufferBuilder *fbb);
// Returns the control loop calculated voltage.
double voltage() const;
@@ -53,11 +57,11 @@
DISALLOW_COPY_AND_ASSIGN(ShooterSide);
};
-class Shooter : public ::aos::controls::ControlLoop<ShooterQueue> {
+class Shooter
+ : public ::aos::controls::ControlLoop<Goal, Position, Status, Output> {
public:
- explicit Shooter(
- ::aos::EventLoop *event_loop,
- const ::std::string &name = ".y2016.control_loops.shooter.shooter_queue");
+ explicit Shooter(::aos::EventLoop *event_loop,
+ const ::std::string &name = "/shooter");
enum class ShooterLatchState {
// Any shoot commands will be passed through without modification.
@@ -73,10 +77,9 @@
};
protected:
- void RunIteration(const ShooterQueue::Goal *goal,
- const ShooterQueue::Position *position,
- ShooterQueue::Output *output,
- ShooterQueue::Status *status) override;
+ void RunIteration(const Goal *goal, const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) override;
private:
ShooterSide left_, right_;
diff --git a/y2016/control_loops/shooter/shooter.q b/y2016/control_loops/shooter/shooter.q
deleted file mode 100644
index 583b762..0000000
--- a/y2016/control_loops/shooter/shooter.q
+++ /dev/null
@@ -1,81 +0,0 @@
-package y2016.control_loops.shooter;
-
-import "aos/controls/control_loops.q";
-import "frc971/control_loops/control_loops.q";
-
-struct ShooterSideStatus {
- // True if the shooter side is up to speed and stable.
- bool ready;
- // The current average velocity in radians/second.
- double avg_angular_velocity;
- // The current instantaneous filtered velocity in radians/second.
- double angular_velocity;
-};
-
-// Published on ".y2016.control_loops.shooter.shooter_queue"
-queue_group ShooterQueue {
- implements aos.control_loops.ControlLoop;
-
- // All angles are in radians, and angular velocities are in radians/second.
- // For all angular velocities, positive is shooting the ball out of the robot.
-
- message Goal {
- // Angular velocity goals in radians/second.
- double angular_velocity;
-
- bool clamp_open; // True to release our clamp on the ball.
- // True to push the ball into the shooter.
- // If we are in the act of shooting with a goal velocity != 0, wait until it
- // is up to speed, push the ball into the shooter, and then wait until it
- // spins up and down before letting the piston be released.
- bool push_to_shooter;
-
- // Forces the lights on.
- bool force_lights_on;
-
- // If true, the robot is shooting forwards.
- bool shooting_forwards;
- };
-
- message Position {
- // Wheel angle in radians/second.
- double theta_left;
- double theta_right;
- };
-
- message Status {
- // Left side status.
- ShooterSideStatus left;
- // Right side status.
- ShooterSideStatus right;
-
- // True if the shooter is ready. It is better to compare the velocities
- // directly so there isn't confusion on if the goal is up to date.
- bool ready;
-
- // The number of shots that have been fired since the start of the shooter
- // control loop.
- uint32_t shots;
- };
-
- message Output {
- // Voltage in volts of the left and right shooter motors.
- double voltage_left;
- double voltage_right;
-
- // See comments on the identical fields in Goal for details.
- bool clamp_open;
- bool push_to_shooter;
-
- // If true, the lights are on.
- bool lights_on;
-
- bool forwards_flashlight;
- bool backwards_flashlight;
- };
-
- queue Goal goal;
- queue Position position;
- queue Output output;
- queue Status status;
-};
diff --git a/y2016/control_loops/shooter/shooter_goal.fbs b/y2016/control_loops/shooter/shooter_goal.fbs
new file mode 100644
index 0000000..f041503
--- /dev/null
+++ b/y2016/control_loops/shooter/shooter_goal.fbs
@@ -0,0 +1,23 @@
+namespace y2016.control_loops.shooter;
+
+// All angles are in radians, and angular velocities are in radians/second.
+// For all angular velocities, positive is shooting the ball out of the robot.
+table Goal {
+ // Angular velocity goals in radians/second.
+ angular_velocity:double;
+
+ clamp_open:bool; // True to release our clamp on the ball.
+ // True to push the ball into the shooter.
+ // If we are in the act of shooting with a goal velocity != 0, wait until it
+ // is up to speed, push the ball into the shooter, and then wait until it
+ // spins up and down before letting the piston be released.
+ push_to_shooter:bool;
+
+ // Forces the lights on.
+ force_lights_on:bool;
+
+ // If true, the robot is shooting forwards.
+ shooting_forwards:bool;
+}
+
+root_type Goal;
diff --git a/y2016/control_loops/shooter/shooter_lib_test.cc b/y2016/control_loops/shooter/shooter_lib_test.cc
index 30b5166..b432049 100644
--- a/y2016/control_loops/shooter/shooter_lib_test.cc
+++ b/y2016/control_loops/shooter/shooter_lib_test.cc
@@ -5,13 +5,15 @@
#include <chrono>
#include <memory>
-#include "gtest/gtest.h"
-#include "aos/queue.h"
#include "aos/controls/control_loop_test.h"
#include "frc971/control_loops/team_number_test_environment.h"
-#include "y2016/control_loops/shooter/shooter.q.h"
+#include "gtest/gtest.h"
#include "y2016/control_loops/shooter/shooter.h"
+#include "y2016/control_loops/shooter/shooter_goal_generated.h"
+#include "y2016/control_loops/shooter/shooter_output_generated.h"
#include "y2016/control_loops/shooter/shooter_plant.h"
+#include "y2016/control_loops/shooter/shooter_position_generated.h"
+#include "y2016/control_loops/shooter/shooter_status_generated.h"
using ::frc971::control_loops::testing::kTeamNumber;
@@ -50,11 +52,8 @@
// Constructs a shooter simulation.
ShooterSimulation(::aos::EventLoop *event_loop, chrono::nanoseconds dt)
: event_loop_(event_loop),
- shooter_position_sender_(
- event_loop_->MakeSender<ShooterQueue::Position>(
- ".y2016.control_loops.shooter.shooter_queue.position")),
- shooter_output_fetcher_(event_loop_->MakeFetcher<ShooterQueue::Output>(
- ".y2016.control_loops.shooter.shooter_queue.output")),
+ shooter_position_sender_(event_loop_->MakeSender<Position>("/shooter")),
+ shooter_output_fetcher_(event_loop_->MakeFetcher<Output>("/shooter")),
shooter_plant_left_(new ShooterPlant(
::y2016::control_loops::shooter::MakeShooterPlant())),
shooter_plant_right_(new ShooterPlant(
@@ -73,12 +72,14 @@
// Sends a queue message with the position of the shooter.
void SendPositionMessage() {
- ::aos::Sender<ShooterQueue::Position>::Message position =
- shooter_position_sender_.MakeMessage();
+ ::aos::Sender<Position>::Builder builder =
+ shooter_position_sender_.MakeBuilder();
- position->theta_left = shooter_plant_left_->Y(0, 0);
- position->theta_right = shooter_plant_right_->Y(0, 0);
- position.Send();
+ Position::Builder position_builder = builder.MakeBuilder<Position>();
+
+ position_builder.add_theta_left(shooter_plant_left_->Y(0, 0));
+ position_builder.add_theta_right(shooter_plant_right_->Y(0, 0));
+ builder.Send(position_builder.Finish());
}
void set_left_voltage_offset(double voltage_offset) {
@@ -95,9 +96,9 @@
::Eigen::Matrix<double, 1, 1> U_left;
::Eigen::Matrix<double, 1, 1> U_right;
- U_left(0, 0) = shooter_output_fetcher_->voltage_left +
+ U_left(0, 0) = shooter_output_fetcher_->voltage_left() +
shooter_plant_left_->voltage_offset();
- U_right(0, 0) = shooter_output_fetcher_->voltage_right +
+ U_right(0, 0) = shooter_output_fetcher_->voltage_right() +
shooter_plant_right_->voltage_offset();
shooter_plant_left_->Update(U_left);
@@ -107,8 +108,8 @@
private:
::aos::EventLoop *event_loop_;
- ::aos::Sender<ShooterQueue::Position> shooter_position_sender_;
- ::aos::Fetcher<ShooterQueue::Output> shooter_output_fetcher_;
+ ::aos::Sender<Position> shooter_position_sender_;
+ ::aos::Fetcher<Output> shooter_output_fetcher_;
::std::unique_ptr<ShooterPlant> shooter_plant_left_, shooter_plant_right_;
@@ -118,18 +119,16 @@
class ShooterTest : public ::aos::testing::ControlLoopTest {
protected:
ShooterTest()
- : ::aos::testing::ControlLoopTest(chrono::microseconds(5000)),
+ : ::aos::testing::ControlLoopTest(
+ aos::configuration::ReadConfig("y2016/config.json"),
+ chrono::microseconds(5000)),
test_event_loop_(MakeEventLoop()),
- shooter_goal_fetcher_(test_event_loop_->MakeFetcher<ShooterQueue::Goal>(
- ".y2016.control_loops.shooter.shooter_queue.goal")),
- shooter_goal_sender_(test_event_loop_->MakeSender<ShooterQueue::Goal>(
- ".y2016.control_loops.shooter.shooter_queue.goal")),
+ shooter_goal_fetcher_(test_event_loop_->MakeFetcher<Goal>("/shooter")),
+ shooter_goal_sender_(test_event_loop_->MakeSender<Goal>("/shooter")),
shooter_status_fetcher_(
- test_event_loop_->MakeFetcher<ShooterQueue::Status>(
- ".y2016.control_loops.shooter.shooter_queue.status")),
+ test_event_loop_->MakeFetcher<Status>("/shooter")),
shooter_output_fetcher_(
- test_event_loop_->MakeFetcher<ShooterQueue::Output>(
- ".y2016.control_loops.shooter.shooter_queue.output")),
+ test_event_loop_->MakeFetcher<Output>("/shooter")),
shooter_event_loop_(MakeEventLoop()),
shooter_(shooter_event_loop_.get()),
shooter_plant_event_loop_(MakeEventLoop()),
@@ -144,22 +143,22 @@
EXPECT_TRUE(shooter_goal_fetcher_.get() != nullptr);
EXPECT_TRUE(shooter_status_fetcher_.get() != nullptr);
- EXPECT_NEAR(shooter_goal_fetcher_->angular_velocity,
- shooter_status_fetcher_->left.angular_velocity, 10.0);
- EXPECT_NEAR(shooter_goal_fetcher_->angular_velocity,
- shooter_status_fetcher_->right.angular_velocity, 10.0);
+ EXPECT_NEAR(shooter_goal_fetcher_->angular_velocity(),
+ shooter_status_fetcher_->left()->angular_velocity(), 10.0);
+ EXPECT_NEAR(shooter_goal_fetcher_->angular_velocity(),
+ shooter_status_fetcher_->right()->angular_velocity(), 10.0);
- EXPECT_NEAR(shooter_goal_fetcher_->angular_velocity,
- shooter_status_fetcher_->left.avg_angular_velocity, 10.0);
- EXPECT_NEAR(shooter_goal_fetcher_->angular_velocity,
- shooter_status_fetcher_->right.avg_angular_velocity, 10.0);
+ EXPECT_NEAR(shooter_goal_fetcher_->angular_velocity(),
+ shooter_status_fetcher_->left()->avg_angular_velocity(), 10.0);
+ EXPECT_NEAR(shooter_goal_fetcher_->angular_velocity(),
+ shooter_status_fetcher_->right()->avg_angular_velocity(), 10.0);
}
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
- ::aos::Fetcher<ShooterQueue::Goal> shooter_goal_fetcher_;
- ::aos::Sender<ShooterQueue::Goal> shooter_goal_sender_;
- ::aos::Fetcher<ShooterQueue::Status> shooter_status_fetcher_;
- ::aos::Fetcher<ShooterQueue::Output> shooter_output_fetcher_;
+ ::aos::Fetcher<Goal> shooter_goal_fetcher_;
+ ::aos::Sender<Goal> shooter_goal_sender_;
+ ::aos::Fetcher<Status> shooter_status_fetcher_;
+ ::aos::Fetcher<Output> shooter_output_fetcher_;
// Create a control loop and simulation.
::std::unique_ptr<::aos::EventLoop> shooter_event_loop_;
@@ -173,9 +172,10 @@
TEST_F(ShooterTest, DoesNothing) {
SetEnabled(true);
{
- auto message = shooter_goal_sender_.MakeMessage();
- message->angular_velocity = 0.0;
- EXPECT_TRUE(message.Send());
+ auto builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angular_velocity(0.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(dt());
@@ -183,8 +183,8 @@
VerifyNearGoal();
EXPECT_TRUE(shooter_output_fetcher_.Fetch());
- EXPECT_EQ(shooter_output_fetcher_->voltage_left, 0.0);
- EXPECT_EQ(shooter_output_fetcher_->voltage_right, 0.0);
+ EXPECT_EQ(shooter_output_fetcher_->voltage_left(), 0.0);
+ EXPECT_EQ(shooter_output_fetcher_->voltage_right(), 0.0);
}
// Tests that the shooter spins up to speed and that it then spins down
@@ -193,32 +193,34 @@
SetEnabled(true);
// Spin up.
{
- auto message = shooter_goal_sender_.MakeMessage();
- message->angular_velocity = 450.0;
- EXPECT_TRUE(message.Send());
+ auto builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angular_velocity(450.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(1));
VerifyNearGoal();
shooter_status_fetcher_.Fetch();
- EXPECT_TRUE(shooter_status_fetcher_->ready);
+ EXPECT_TRUE(shooter_status_fetcher_->ready());
{
- auto message = shooter_goal_sender_.MakeMessage();
- message->angular_velocity = 0.0;
- EXPECT_TRUE(message.Send());
+ auto builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angular_velocity(0.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
// Make sure we don't apply voltage on spin-down.
RunFor(dt());
EXPECT_TRUE(shooter_output_fetcher_.Fetch());
- EXPECT_EQ(0.0, shooter_output_fetcher_->voltage_left);
- EXPECT_EQ(0.0, shooter_output_fetcher_->voltage_right);
+ EXPECT_EQ(0.0, shooter_output_fetcher_->voltage_left());
+ EXPECT_EQ(0.0, shooter_output_fetcher_->voltage_right());
// Continue to stop.
RunFor(chrono::seconds(5));
EXPECT_TRUE(shooter_output_fetcher_.Fetch());
- EXPECT_EQ(0.0, shooter_output_fetcher_->voltage_left);
- EXPECT_EQ(0.0, shooter_output_fetcher_->voltage_right);
+ EXPECT_EQ(0.0, shooter_output_fetcher_->voltage_left());
+ EXPECT_EQ(0.0, shooter_output_fetcher_->voltage_right());
}
// Tests that the shooter doesn't say it is ready if one side isn't up to speed.
@@ -228,9 +230,10 @@
SetEnabled(true);
// Spin up.
{
- auto message = shooter_goal_sender_.MakeMessage();
- message->angular_velocity = 20.0;
- EXPECT_TRUE(message.Send());
+ auto builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angular_velocity(20.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
// Cause problems by adding a voltage error on one side.
shooter_plant_.set_right_voltage_offset(-4.0);
@@ -243,9 +246,9 @@
EXPECT_TRUE(shooter_status_fetcher_.get() != nullptr);
// Left should be up to speed, right shouldn't.
- EXPECT_TRUE(shooter_status_fetcher_->left.ready);
- EXPECT_FALSE(shooter_status_fetcher_->right.ready);
- EXPECT_FALSE(shooter_status_fetcher_->ready);
+ EXPECT_TRUE(shooter_status_fetcher_->left()->ready());
+ EXPECT_FALSE(shooter_status_fetcher_->right()->ready());
+ EXPECT_FALSE(shooter_status_fetcher_->ready());
RunFor(chrono::seconds(5));
@@ -256,22 +259,23 @@
EXPECT_TRUE(shooter_status_fetcher_.get() != nullptr);
// Both should be up to speed.
- EXPECT_TRUE(shooter_status_fetcher_->left.ready);
- EXPECT_TRUE(shooter_status_fetcher_->right.ready);
- EXPECT_TRUE(shooter_status_fetcher_->ready);
+ EXPECT_TRUE(shooter_status_fetcher_->left()->ready());
+ EXPECT_TRUE(shooter_status_fetcher_->right()->ready());
+ EXPECT_TRUE(shooter_status_fetcher_->ready());
}
// Tests that the shooter can spin up nicely after being disabled for a while.
TEST_F(ShooterTest, Disabled) {
{
- auto message = shooter_goal_sender_.MakeMessage();
- message->angular_velocity = 200.0;
- EXPECT_TRUE(message.Send());
+ auto builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angular_velocity(200.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
EXPECT_TRUE(shooter_output_fetcher_.Fetch());
- EXPECT_EQ(shooter_output_fetcher_->voltage_left, 0.0);
- EXPECT_EQ(shooter_output_fetcher_->voltage_right, 0.0);
+ EXPECT_EQ(shooter_output_fetcher_->voltage_left(), 0.0);
+ EXPECT_EQ(shooter_output_fetcher_->voltage_right(), 0.0);
SetEnabled(true);
RunFor(chrono::seconds(5));
diff --git a/y2016/control_loops/shooter/shooter_main.cc b/y2016/control_loops/shooter/shooter_main.cc
index b9737fd..6e46114 100644
--- a/y2016/control_loops/shooter/shooter_main.cc
+++ b/y2016/control_loops/shooter/shooter_main.cc
@@ -1,12 +1,15 @@
#include "y2016/control_loops/shooter/shooter.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
int main() {
::aos::InitNRT(true);
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::y2016::control_loops::shooter::Shooter shooter(&event_loop);
event_loop.Run();
diff --git a/y2016/control_loops/shooter/shooter_output.fbs b/y2016/control_loops/shooter/shooter_output.fbs
new file mode 100644
index 0000000..6d7fcdf
--- /dev/null
+++ b/y2016/control_loops/shooter/shooter_output.fbs
@@ -0,0 +1,19 @@
+namespace y2016.control_loops.shooter;
+
+table Output {
+ // Voltage in volts of the left and right shooter motors.
+ voltage_left:double;
+ voltage_right:double;
+
+ // See comments on the identical fields in Goal for details.
+ clamp_open:bool;
+ push_to_shooter:bool;
+
+ // If true, the lights are on.
+ lights_on:bool;
+
+ forwards_flashlight:bool;
+ backwards_flashlight:bool;
+}
+
+root_type Output;
diff --git a/y2016/control_loops/shooter/shooter_position.fbs b/y2016/control_loops/shooter/shooter_position.fbs
new file mode 100644
index 0000000..d97268c
--- /dev/null
+++ b/y2016/control_loops/shooter/shooter_position.fbs
@@ -0,0 +1,11 @@
+namespace y2016.control_loops.shooter;
+
+// All angles are in radians, and angular velocities are in radians/second.
+// For all angular velocities, positive is shooting the ball out of the robot.
+table Position {
+ // Wheel angle in radians/second.
+ theta_left:double;
+ theta_right:double;
+}
+
+root_type Position;
diff --git a/y2016/control_loops/shooter/shooter_status.fbs b/y2016/control_loops/shooter/shooter_status.fbs
new file mode 100644
index 0000000..6f6262b
--- /dev/null
+++ b/y2016/control_loops/shooter/shooter_status.fbs
@@ -0,0 +1,27 @@
+namespace y2016.control_loops.shooter;
+
+table ShooterSideStatus {
+ // True if the shooter side is up to speed and stable.
+ ready:bool;
+ // The current average velocity in radians/second.
+ avg_angular_velocity:double;
+ // The current instantaneous filtered velocity in radians/second.
+ angular_velocity:double;
+}
+
+table Status {
+ // Left side status.
+ left:ShooterSideStatus;
+ // Right side status.
+ right:ShooterSideStatus;
+
+ // True if the shooter is ready. It is better to compare the velocities
+ // directly so there isn't confusion on if the goal is up to date.
+ ready:bool;
+
+ // The number of shots that have been fired since the start of the shooter
+ // control loop.
+ shots:uint;
+}
+
+root_type Status;
diff --git a/y2016/control_loops/superstructure/BUILD b/y2016/control_loops/superstructure/BUILD
index 6a14a5f..4048cdc 100644
--- a/y2016/control_loops/superstructure/BUILD
+++ b/y2016/control_loops/superstructure/BUILD
@@ -1,15 +1,42 @@
package(default_visibility = ["//visibility:public"])
-load("//aos/build:queues.bzl", "queue_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
-queue_library(
- name = "superstructure_queue",
+flatbuffer_cc_library(
+ name = "superstructure_goal_fbs",
srcs = [
- "superstructure.q",
+ "superstructure_goal.fbs",
],
- deps = [
- "//aos/controls:control_loop_queues",
- "//frc971/control_loops:queues",
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "superstructure_position_fbs",
+ srcs = [
+ "superstructure_position.fbs",
+ ],
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "superstructure_output_fbs",
+ srcs = [
+ "superstructure_output.fbs",
+ ],
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "superstructure_status_fbs",
+ srcs = [
+ "superstructure_status.fbs",
+ ],
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
],
)
@@ -73,8 +100,11 @@
"superstructure_controls.h",
],
deps = [
+ ":superstructure_goal_fbs",
+ ":superstructure_output_fbs",
":superstructure_plants",
- ":superstructure_queue",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
"//aos:math",
"//aos/controls:control_loop",
"//aos/util:trapezoid_profile",
@@ -83,7 +113,7 @@
"//frc971/control_loops:state_feedback_loop",
"//frc971/zeroing",
"//y2016:constants",
- "//y2016/queues:ball_detector",
+ "//y2016/queues:ball_detector_fbs",
],
)
@@ -92,11 +122,14 @@
srcs = [
"superstructure_lib_test.cc",
],
+ data = ["//y2016:config.json"],
deps = [
+ ":superstructure_goal_fbs",
":superstructure_lib",
- ":superstructure_queue",
+ ":superstructure_output_fbs",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
"//aos:math",
- "//aos:queues",
"//aos/controls:control_loop_test",
"//aos/testing:googletest",
"//aos/time",
@@ -112,8 +145,7 @@
],
deps = [
":superstructure_lib",
- ":superstructure_queue",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
],
)
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index 99a130c..8f4d02e 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -1,13 +1,12 @@
#include "y2016/control_loops/superstructure/superstructure.h"
-#include "y2016/control_loops/superstructure/superstructure_controls.h"
#include "aos/commonmath.h"
-#include "aos/controls/control_loops.q.h"
#include "aos/logging/logging.h"
-#include "y2016/control_loops/superstructure/integral_intake_plant.h"
#include "y2016/control_loops/superstructure/integral_arm_plant.h"
-#include "y2016/queues/ball_detector.q.h"
+#include "y2016/control_loops/superstructure/integral_intake_plant.h"
+#include "y2016/control_loops/superstructure/superstructure_controls.h"
+#include "y2016/queues/ball_detector_generated.h"
#include "y2016/constants.h"
@@ -229,11 +228,11 @@
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),
ball_detector_fetcher_(
event_loop->MakeFetcher<::y2016::sensors::BallDetector>(
- ".y2016.sensors.ball_detector")),
+ "/superstructure")),
collision_avoidance_(&intake_, &arm_) {}
bool Superstructure::IsArmNear(double shoulder_tolerance,
@@ -289,10 +288,10 @@
}
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) {
+ const Goal *unsafe_goal,
+ const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) {
const State state_before_switch = state_;
if (WasReset()) {
AOS_LOG(ERROR, "WPILib reset, restarting\n");
@@ -304,8 +303,8 @@
// Bool to track if we should turn the motors on or not.
bool disable = output == nullptr;
- arm_.Correct(position->shoulder, position->wrist);
- intake_.Correct(position->intake);
+ arm_.Correct(position->shoulder(), position->wrist());
+ intake_.Correct(*position->intake());
// There are 2 main zeroing paths, HIGH_ARM_ZERO and LOW_ARM_ZERO.
//
@@ -571,14 +570,14 @@
4.0, // Shoulder acceleration,
4.0, // Wrist velocity
10.0); // Wrist acceleration.
- intake_.AdjustProfile(unsafe_goal->max_angular_velocity_intake,
- unsafe_goal->max_angular_acceleration_intake);
+ intake_.AdjustProfile(unsafe_goal->max_angular_velocity_intake(),
+ unsafe_goal->max_angular_acceleration_intake());
requested_shoulder =
- ::std::max(unsafe_goal->angle_shoulder,
+ ::std::max(unsafe_goal->angle_shoulder(),
constants::Values::kShoulderRange.lower);
requested_wrist = 0.0;
- requested_intake = unsafe_goal->angle_intake;
+ requested_intake = unsafe_goal->angle_intake();
// Transition to landing once the profile is close to finished for the
// shoulder.
if (arm_.goal(0, 0) > kShoulderTransitionToLanded + 1e-4 ||
@@ -591,18 +590,18 @@
}
} else {
// Otherwise, give the user what he asked for.
- arm_.AdjustProfile(unsafe_goal->max_angular_velocity_shoulder,
- unsafe_goal->max_angular_acceleration_shoulder,
- unsafe_goal->max_angular_velocity_wrist,
- unsafe_goal->max_angular_acceleration_wrist);
- intake_.AdjustProfile(unsafe_goal->max_angular_velocity_intake,
- unsafe_goal->max_angular_acceleration_intake);
+ arm_.AdjustProfile(unsafe_goal->max_angular_velocity_shoulder(),
+ unsafe_goal->max_angular_acceleration_shoulder(),
+ unsafe_goal->max_angular_velocity_wrist(),
+ unsafe_goal->max_angular_acceleration_wrist());
+ intake_.AdjustProfile(unsafe_goal->max_angular_velocity_intake(),
+ unsafe_goal->max_angular_acceleration_intake());
// Except, don't let the shoulder go all the way down.
- requested_shoulder = ::std::max(unsafe_goal->angle_shoulder,
+ requested_shoulder = ::std::max(unsafe_goal->angle_shoulder(),
kShoulderTransitionToLanded);
- requested_wrist = unsafe_goal->angle_wrist;
- requested_intake = unsafe_goal->angle_intake;
+ requested_wrist = unsafe_goal->angle_wrist();
+ requested_intake = unsafe_goal->angle_intake();
// Transition to landing once the profile is close to finished for the
// shoulder.
@@ -641,9 +640,9 @@
if (unsafe_goal) {
constexpr float kTriggerThreshold = 12.0 * 0.90 / 0.005;
- if (unsafe_goal->voltage_climber > 1.0) {
+ if (unsafe_goal->voltage_climber() > 1.0) {
kill_shoulder_accumulator_ +=
- ::std::min(kMaxClimberVoltage, unsafe_goal->voltage_climber);
+ ::std::min(kMaxClimberVoltage, unsafe_goal->voltage_climber());
} else {
kill_shoulder_accumulator_ = 0.0;
}
@@ -673,24 +672,28 @@
}
// Calculate the loops for a cycle.
+ double intake_position_power;
+ double intake_velocity_power;
{
Eigen::Matrix<double, 3, 1> error = intake_.controller().error();
- status->intake.position_power =
+ intake_position_power =
intake_.controller().controller().K(0, 0) * error(0, 0);
- status->intake.velocity_power =
+ intake_velocity_power =
intake_.controller().controller().K(0, 1) * error(1, 0);
}
+ double shoulder_position_power;
+ double shoulder_velocity_power;
+ double wrist_position_power;
+ double wrist_velocity_power;
{
Eigen::Matrix<double, 6, 1> error = arm_.controller().error();
- status->shoulder.position_power =
+ shoulder_position_power =
arm_.controller().controller().K(0, 0) * error(0, 0);
- status->shoulder.velocity_power =
+ shoulder_velocity_power =
arm_.controller().controller().K(0, 1) * error(1, 0);
- status->wrist.position_power =
- arm_.controller().controller().K(0, 2) * error(2, 0);
- status->wrist.velocity_power =
- arm_.controller().controller().K(0, 3) * error(3, 0);
+ wrist_position_power = arm_.controller().controller().K(0, 2) * error(2, 0);
+ wrist_velocity_power = arm_.controller().controller().K(0, 3) * error(3, 0);
}
arm_.Update(disable);
@@ -698,98 +701,135 @@
// Write out all the voltages.
if (output) {
- output->voltage_intake = intake_.voltage();
- output->voltage_shoulder = arm_.shoulder_voltage();
- output->voltage_wrist = arm_.wrist_voltage();
+ OutputT output_struct;
+ output_struct.voltage_intake = intake_.voltage();
+ output_struct.voltage_shoulder = arm_.shoulder_voltage();
+ output_struct.voltage_wrist = arm_.wrist_voltage();
- output->voltage_top_rollers = 0.0;
- output->voltage_bottom_rollers = 0.0;
+ output_struct.voltage_top_rollers = 0.0;
+ output_struct.voltage_bottom_rollers = 0.0;
- output->voltage_climber = 0.0;
- output->unfold_climber = false;
+ output_struct.voltage_climber = 0.0;
+ output_struct.unfold_climber = false;
if (unsafe_goal) {
// Ball detector lights.
ball_detector_fetcher_.Fetch();
bool ball_detected = false;
if (ball_detector_fetcher_.get()) {
- ball_detected = ball_detector_fetcher_->voltage > 2.5;
+ ball_detected = ball_detector_fetcher_->voltage() > 2.5;
}
// Climber.
- output->voltage_climber = ::std::max(
+ output_struct.voltage_climber = ::std::max(
static_cast<float>(0.0),
- ::std::min(unsafe_goal->voltage_climber, kMaxClimberVoltage));
- output->unfold_climber = unsafe_goal->unfold_climber;
+ ::std::min(unsafe_goal->voltage_climber(), kMaxClimberVoltage));
+ output_struct.unfold_climber = unsafe_goal->unfold_climber();
// Intake.
- if (unsafe_goal->force_intake || !ball_detected) {
- output->voltage_top_rollers = ::std::max(
+ if (unsafe_goal->force_intake() || !ball_detected) {
+ output_struct.voltage_top_rollers = ::std::max(
-kMaxIntakeTopVoltage,
- ::std::min(unsafe_goal->voltage_top_rollers, kMaxIntakeTopVoltage));
- output->voltage_bottom_rollers =
+ ::std::min(unsafe_goal->voltage_top_rollers(), kMaxIntakeTopVoltage));
+ output_struct.voltage_bottom_rollers =
::std::max(-kMaxIntakeBottomVoltage,
- ::std::min(unsafe_goal->voltage_bottom_rollers,
+ ::std::min(unsafe_goal->voltage_bottom_rollers(),
kMaxIntakeBottomVoltage));
} else {
- output->voltage_top_rollers = 0.0;
- output->voltage_bottom_rollers = 0.0;
+ output_struct.voltage_top_rollers = 0.0;
+ output_struct.voltage_bottom_rollers = 0.0;
}
// Traverse.
- output->traverse_unlatched = unsafe_goal->traverse_unlatched;
- output->traverse_down = unsafe_goal->traverse_down;
+ output_struct.traverse_unlatched = unsafe_goal->traverse_unlatched();
+ output_struct.traverse_down = unsafe_goal->traverse_down();
}
+
+ output->Send(Output::Pack(*output->fbb(), &output_struct));
}
// Save debug/internal state.
- status->zeroed = arm_.zeroed() && intake_.zeroed();
+ flatbuffers::Offset<frc971::EstimatorState> shoulder_estimator_state_offset =
+ arm_.EstimatorState(status->fbb(), 0);
- status->shoulder.angle = arm_.X_hat(0, 0);
- status->shoulder.angular_velocity = arm_.X_hat(1, 0);
- status->shoulder.goal_angle = arm_.goal(0, 0);
- status->shoulder.goal_angular_velocity = arm_.goal(1, 0);
- status->shoulder.unprofiled_goal_angle = arm_.unprofiled_goal(0, 0);
- status->shoulder.unprofiled_goal_angular_velocity =
- arm_.unprofiled_goal(1, 0);
- status->shoulder.voltage_error = arm_.X_hat(4, 0);
- status->shoulder.calculated_velocity =
- (arm_.shoulder_angle() - last_shoulder_angle_) / 0.005;
- status->shoulder.estimator_state = arm_.EstimatorState(0);
+ JointState::Builder shoulder_builder = status->MakeBuilder<JointState>();
- status->wrist.angle = arm_.X_hat(2, 0);
- status->wrist.angular_velocity = arm_.X_hat(3, 0);
- status->wrist.goal_angle = arm_.goal(2, 0);
- status->wrist.goal_angular_velocity = arm_.goal(3, 0);
- status->wrist.unprofiled_goal_angle = arm_.unprofiled_goal(2, 0);
- status->wrist.unprofiled_goal_angular_velocity = arm_.unprofiled_goal(3, 0);
- status->wrist.voltage_error = arm_.X_hat(5, 0);
- status->wrist.calculated_velocity =
- (arm_.wrist_angle() - last_wrist_angle_) / 0.005;
- status->wrist.estimator_state = arm_.EstimatorState(1);
+ shoulder_builder.add_angle(arm_.X_hat(0, 0));
+ shoulder_builder.add_angular_velocity(arm_.X_hat(1, 0));
+ shoulder_builder.add_goal_angle(arm_.goal(0, 0));
+ shoulder_builder.add_goal_angular_velocity(arm_.goal(1, 0));
+ shoulder_builder.add_unprofiled_goal_angle(arm_.unprofiled_goal(0, 0));
+ shoulder_builder.add_unprofiled_goal_angular_velocity(
+ arm_.unprofiled_goal(1, 0));
+ shoulder_builder.add_voltage_error(arm_.X_hat(4, 0));
+ shoulder_builder.add_calculated_velocity(
+ (arm_.shoulder_angle() - last_shoulder_angle_) / 0.005);
+ shoulder_builder.add_position_power(shoulder_position_power);
+ shoulder_builder.add_velocity_power(shoulder_velocity_power);
+ shoulder_builder.add_estimator_state(shoulder_estimator_state_offset);
- status->intake.angle = intake_.X_hat(0, 0);
- status->intake.angular_velocity = intake_.X_hat(1, 0);
- status->intake.goal_angle = intake_.goal(0, 0);
- status->intake.goal_angular_velocity = intake_.goal(1, 0);
- status->intake.unprofiled_goal_angle = intake_.unprofiled_goal(0, 0);
- status->intake.unprofiled_goal_angular_velocity =
- intake_.unprofiled_goal(1, 0);
- status->intake.calculated_velocity =
- (intake_.position() - last_intake_angle_) / 0.005;
- status->intake.voltage_error = intake_.X_hat(2, 0);
- status->intake.estimator_state = intake_.EstimatorState(0);
- status->intake.feedforwards_power = intake_.controller().ff_U(0, 0);
+ flatbuffers::Offset<JointState> shoulder_offset = shoulder_builder.Finish();
- status->shoulder_controller_index = arm_.controller_index();
+ flatbuffers::Offset<frc971::EstimatorState> wrist_estimator_state_offset =
+ arm_.EstimatorState(status->fbb(), 1);
+
+ JointState::Builder wrist_builder = status->MakeBuilder<JointState>();
+
+ wrist_builder.add_angle(arm_.X_hat(2, 0));
+ wrist_builder.add_angular_velocity(arm_.X_hat(3, 0));
+ wrist_builder.add_goal_angle(arm_.goal(2, 0));
+ wrist_builder.add_goal_angular_velocity(arm_.goal(3, 0));
+ wrist_builder.add_unprofiled_goal_angle(arm_.unprofiled_goal(2, 0));
+ wrist_builder.add_unprofiled_goal_angular_velocity(
+ arm_.unprofiled_goal(3, 0));
+ wrist_builder.add_voltage_error(arm_.X_hat(5, 0));
+ wrist_builder.add_calculated_velocity(
+ (arm_.wrist_angle() - last_wrist_angle_) / 0.005);
+ wrist_builder.add_position_power(wrist_position_power);
+ wrist_builder.add_velocity_power(wrist_velocity_power);
+ wrist_builder.add_estimator_state(wrist_estimator_state_offset);
+
+ flatbuffers::Offset<JointState> wrist_offset = wrist_builder.Finish();
+
+ flatbuffers::Offset<frc971::EstimatorState> intake_estimator_state_offset =
+ intake_.EstimatorState(status->fbb(), 0);
+
+ JointState::Builder intake_builder = status->MakeBuilder<JointState>();
+ intake_builder.add_position_power(intake_position_power);
+ intake_builder.add_velocity_power(intake_velocity_power);
+ intake_builder.add_angle(intake_.X_hat(0, 0));
+ intake_builder.add_angular_velocity(intake_.X_hat(1, 0));
+ intake_builder.add_goal_angle(intake_.goal(0, 0));
+ intake_builder.add_goal_angular_velocity(intake_.goal(1, 0));
+ intake_builder.add_unprofiled_goal_angle(intake_.unprofiled_goal(0, 0));
+ intake_builder.add_unprofiled_goal_angular_velocity(
+ intake_.unprofiled_goal(1, 0));
+ intake_builder.add_calculated_velocity(
+ (intake_.position() - last_intake_angle_) / 0.005);
+ intake_builder.add_voltage_error(intake_.X_hat(2, 0));
+ intake_builder.add_estimator_state(intake_estimator_state_offset);
+ intake_builder.add_feedforwards_power(intake_.controller().ff_U(0, 0));
+
+ flatbuffers::Offset<JointState> intake_offset = intake_builder.Finish();
+
+ Status::Builder status_builder = status->MakeBuilder<Status>();
+
+ status_builder.add_shoulder(shoulder_offset);
+ status_builder.add_wrist(wrist_offset);
+ status_builder.add_intake(intake_offset);
+
+ status_builder.add_zeroed(arm_.zeroed() && intake_.zeroed());
+ status_builder.add_shoulder_controller_index(arm_.controller_index());
last_shoulder_angle_ = arm_.shoulder_angle();
last_wrist_angle_ = arm_.wrist_angle();
last_intake_angle_ = intake_.position();
- status->estopped = (state_ == ESTOP);
+ status_builder.add_estopped((state_ == ESTOP));
- status->state = state_;
- status->is_collided = is_collided;
+ status_builder.add_state(state_);
+ status_builder.add_is_collided(is_collided);
+
+ status->Send(status_builder.Finish());
last_state_ = state_before_switch;
}
diff --git a/y2016/control_loops/superstructure/superstructure.h b/y2016/control_loops/superstructure/superstructure.h
index 610db8e..2219c95 100644
--- a/y2016/control_loops/superstructure/superstructure.h
+++ b/y2016/control_loops/superstructure/superstructure.h
@@ -8,9 +8,12 @@
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/zeroing/zeroing.h"
-#include "y2016/control_loops/superstructure/superstructure.q.h"
#include "y2016/control_loops/superstructure/superstructure_controls.h"
-#include "y2016/queues/ball_detector.q.h"
+#include "y2016/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2016/queues/ball_detector_generated.h"
namespace y2016 {
namespace control_loops {
@@ -105,11 +108,10 @@
};
class Superstructure
- : public ::aos::controls::ControlLoop<control_loops::SuperstructureQueue> {
+ : public ::aos::controls::ControlLoop<Goal, Position, Status, Output> {
public:
- explicit Superstructure(
- ::aos::EventLoop *event_loop,
- const ::std::string &name = ".y2016.control_loops.superstructure_queue");
+ explicit Superstructure(::aos::EventLoop *event_loop,
+ const ::std::string &name = "/superstructure");
static constexpr double kZeroingVoltage = 6.0;
static constexpr double kShooterHangingVoltage = 6.0;
@@ -209,11 +211,9 @@
bool collided() const { return collision_avoidance_.collided(); }
protected:
- virtual void RunIteration(
- const control_loops::SuperstructureQueue::Goal *unsafe_goal,
- const control_loops::SuperstructureQueue::Position *position,
- control_loops::SuperstructureQueue::Output *output,
- control_loops::SuperstructureQueue::Status *status) override;
+ virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) override;
private:
friend class testing::SuperstructureTest_DisabledGoalTest_Test;
diff --git a/y2016/control_loops/superstructure/superstructure.q b/y2016/control_loops/superstructure/superstructure.q
deleted file mode 100644
index d784929..0000000
--- a/y2016/control_loops/superstructure/superstructure.q
+++ /dev/null
@@ -1,145 +0,0 @@
-package y2016.control_loops;
-
-import "aos/controls/control_loops.q";
-import "frc971/control_loops/control_loops.q";
-
-struct JointState {
- // Angle of the joint in radians.
- float angle;
- // Angular velocity of the joint in radians/second.
- float angular_velocity;
- // Profiled goal angle of the joint in radians.
- float goal_angle;
- // Profiled goal angular velocity of the joint in radians/second.
- float goal_angular_velocity;
- // Unprofiled goal angle of the joint in radians.
- float unprofiled_goal_angle;
- // Unprofiled goal angular velocity of the joint in radians/second.
- float unprofiled_goal_angular_velocity;
-
- // The estimated voltage error.
- float voltage_error;
-
- // The calculated velocity with delta x/delta t
- float calculated_velocity;
-
- // Components of the control loop output
- float position_power;
- float velocity_power;
- float feedforwards_power;
-
- // State of the estimator.
- .frc971.EstimatorState estimator_state;
-};
-
-// Published on ".y2016.control_loops.superstructure_queue"
-queue_group SuperstructureQueue {
- implements aos.control_loops.ControlLoop;
-
- message Goal {
- // Zero on the intake is when the horizontal tube stock members are level
- // with the top frame rails of the robot. This will be essentially when we
- // are in the intaking position. Positive is up. The angle is measured
- // relative to the top
- // of the robot frame.
- // Zero on the shoulder is when the shoulder is down against the hard stop
- // blocks. Positive is up. The angle is measured relative to the top of
- // the robot frame.
- // Zero on the wrist is horizontal and landed in the bellypan. Positive is
- // the same direction as the shoulder. The angle is measured relative to
- // the top of the robot frame. For calibration, 0 is measured as parallel
- // to the big frame supporting the shooter.
-
- // Goal angles and angular velocities of the superstructure subsystems.
- double angle_intake;
- double angle_shoulder;
- // In relation to the ground plane.
- double angle_wrist;
-
- // Caps on velocity/acceleration for profiling. 0 for the default.
- float max_angular_velocity_intake;
- float max_angular_velocity_shoulder;
- float max_angular_velocity_wrist;
-
- float max_angular_acceleration_intake;
- float max_angular_acceleration_shoulder;
- float max_angular_acceleration_wrist;
-
- // Voltage to send to the rollers. Positive is sucking in.
- float voltage_top_rollers;
- float voltage_bottom_rollers;
-
- // Voltage to sent to the climber. Positive is pulling the robot up.
- float voltage_climber;
- // If true, unlatch the climber and allow it to unfold.
- bool unfold_climber;
-
- bool force_intake;
-
- // If true, release the latch which holds the traverse mechanism in the
- // middle.
- bool traverse_unlatched;
- // If true, fire the traverse mechanism down.
- bool traverse_down;
- };
-
- message Status {
- // Are the superstructure subsystems zeroed?
- bool zeroed;
-
- // If true, we have aborted.
- bool estopped;
-
- // The internal state of the state machine.
- int32_t state;
-
-
- // Estimated angles and angular velocities of the superstructure subsystems.
- JointState intake;
- JointState shoulder;
- JointState wrist;
-
- int32_t shoulder_controller_index;
-
- // Is the superstructure collided?
- bool is_collided;
- };
-
- message Position {
- // Zero for the intake potentiometer value is horizontal, and positive is
- // up.
- // Zero for the shoulder potentiometer value is horizontal, and positive is
- // up.
- // Zero for the wrist potentiometer value is parallel to the arm and with
- // the shooter wheels pointed towards the shoulder joint. This is measured
- // relative to the arm, not the ground, not like the world we actually
- // present to users.
- .frc971.PotAndIndexPosition intake;
- .frc971.PotAndIndexPosition shoulder;
- .frc971.PotAndIndexPosition wrist;
- };
-
- message Output {
- float voltage_intake;
- float voltage_shoulder;
- float voltage_wrist;
-
- float voltage_top_rollers;
- float voltage_bottom_rollers;
-
- // Voltage to sent to the climber. Positive is pulling the robot up.
- float voltage_climber;
- // If true, release the latch to trigger the climber to unfold.
- bool unfold_climber;
-
- // If true, release the latch to hold the traverse mechanism in the middle.
- bool traverse_unlatched;
- // If true, fire the traverse mechanism down.
- bool traverse_down;
- };
-
- queue Goal goal;
- queue Position position;
- queue Output output;
- queue Status status;
-};
diff --git a/y2016/control_loops/superstructure/superstructure_controls.cc b/y2016/control_loops/superstructure/superstructure_controls.cc
index b8f3a48..269089b 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.cc
+++ b/y2016/control_loops/superstructure/superstructure_controls.cc
@@ -1,6 +1,5 @@
#include "y2016/control_loops/superstructure/superstructure_controls.h"
-#include "aos/controls/control_loops.q.h"
#include "aos/logging/logging.h"
#include "y2016/control_loops/superstructure/integral_intake_plant.h"
@@ -96,10 +95,10 @@
// TODO(austin): Handle zeroing errors.
-void Arm::Correct(PotAndIndexPosition position_shoulder,
- PotAndIndexPosition position_wrist) {
- estimators_[kShoulderIndex].UpdateEstimate(position_shoulder);
- estimators_[kWristIndex].UpdateEstimate(position_wrist);
+void Arm::Correct(const PotAndIndexPosition *position_shoulder,
+ const PotAndIndexPosition *position_wrist) {
+ estimators_[kShoulderIndex].UpdateEstimate(*position_shoulder);
+ estimators_[kWristIndex].UpdateEstimate(*position_wrist);
// Handle zeroing errors
if (estimators_[kShoulderIndex].error()) {
@@ -130,7 +129,7 @@
}
{
- Y_ << position_shoulder.encoder, position_wrist.encoder;
+ Y_ << position_shoulder->encoder(), position_wrist->encoder();
Y_ += offset_;
loop_->Correct(Y_);
}
diff --git a/y2016/control_loops/superstructure/superstructure_controls.h b/y2016/control_loops/superstructure/superstructure_controls.h
index 8936650..d301054 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.h
+++ b/y2016/control_loops/superstructure/superstructure_controls.h
@@ -11,7 +11,7 @@
#include "frc971/zeroing/zeroing.h"
#include "y2016/control_loops/superstructure/integral_arm_plant.h"
-#include "y2016/control_loops/superstructure/superstructure.q.h"
+#include "y2016/control_loops/superstructure/superstructure_position_generated.h"
namespace y2016 {
namespace control_loops {
@@ -111,8 +111,8 @@
Arm();
// Updates our estimator with the latest position.
- void Correct(::frc971::PotAndIndexPosition position_shoulder,
- ::frc971::PotAndIndexPosition position_wrist);
+ void Correct(const ::frc971::PotAndIndexPosition *position_shoulder,
+ const ::frc971::PotAndIndexPosition *position_wrist);
// Forces the goal to be the provided goal.
void ForceGoal(double unprofiled_goal_shoulder, double unprofiled_goal_wrist);
diff --git a/y2016/control_loops/superstructure/superstructure_goal.fbs b/y2016/control_loops/superstructure/superstructure_goal.fbs
new file mode 100644
index 0000000..4274bd8
--- /dev/null
+++ b/y2016/control_loops/superstructure/superstructure_goal.fbs
@@ -0,0 +1,50 @@
+namespace y2016.control_loops.superstructure;
+
+table Goal {
+ // Zero on the intake is when the horizontal tube stock members are level
+ // with the top frame rails of the robot. This will be essentially when we
+ // are in the intaking position. Positive is up. The angle is measured
+ // relative to the top
+ // of the robot frame.
+ // Zero on the shoulder is when the shoulder is down against the hard stop
+ // blocks. Positive is up. The angle is measured relative to the top of
+ // the robot frame.
+ // Zero on the wrist is horizontal and landed in the bellypan. Positive is
+ // the same direction as the shoulder. The angle is measured relative to
+ // the top of the robot frame. For calibration, 0 is measured as parallel
+ // to the big frame supporting the shooter.
+
+ // Goal angles and angular velocities of the superstructure subsystems.
+ angle_intake:double;
+ angle_shoulder:double;
+ // In relation to the ground plane.
+ angle_wrist:double;
+
+ // Caps on velocity/acceleration for profiling. 0 for the default.
+ max_angular_velocity_intake:float;
+ max_angular_velocity_shoulder:float;
+ max_angular_velocity_wrist:float;
+
+ max_angular_acceleration_intake:float;
+ max_angular_acceleration_shoulder:float;
+ max_angular_acceleration_wrist:float;
+
+ // Voltage to send to the rollers. Positive is sucking in.
+ voltage_top_rollers:float;
+ voltage_bottom_rollers:float;
+
+ // Voltage to sent to the climber. Positive is pulling the robot up.
+ voltage_climber:float;
+ // If true, unlatch the climber and allow it to unfold.
+ unfold_climber:bool;
+
+ force_intake:bool;
+
+ // If true, release the latch which holds the traverse mechanism in the
+ // middle.
+ traverse_unlatched:bool;
+ // If true, fire the traverse mechanism down.
+ traverse_down:bool;
+}
+
+root_type Goal;
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index 79b4d24..d530cb3 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -7,14 +7,16 @@
#include "aos/commonmath.h"
#include "aos/controls/control_loop_test.h"
-#include "aos/queue.h"
#include "aos/time/time.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
#include "y2016/control_loops/superstructure/arm_plant.h"
#include "y2016/control_loops/superstructure/intake_plant.h"
-#include "y2016/control_loops/superstructure/superstructure.q.h"
+#include "y2016/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_status_generated.h"
#include "y2016/constants.h"
@@ -85,14 +87,11 @@
: event_loop_(event_loop),
dt_(dt),
superstructure_position_sender_(
- event_loop_->MakeSender<SuperstructureQueue::Position>(
- ".y2016.control_loops.superstructure_queue.position")),
+ event_loop_->MakeSender<Position>("/superstructure")),
superstructure_status_fetcher_(
- event_loop_->MakeFetcher<SuperstructureQueue::Status>(
- ".y2016.control_loops.superstructure_queue.status")),
+ event_loop_->MakeFetcher<Status>("/superstructure")),
superstructure_output_fetcher_(
- event_loop_->MakeFetcher<SuperstructureQueue::Output>(
- ".y2016.control_loops.superstructure_queue.output")),
+ event_loop_->MakeFetcher<Output>("/superstructure")),
intake_plant_(new IntakePlant(MakeIntakePlant())),
arm_plant_(new ArmPlant(MakeArmPlant())),
pot_encoder_intake_(constants::Values::kIntakeEncoderIndexDifference),
@@ -144,14 +143,32 @@
// Sends a queue message with the position.
void SendPositionMessage() {
- ::aos::Sender<control_loops::SuperstructureQueue::Position>::Message
- position = superstructure_position_sender_.MakeMessage();
+ ::aos::Sender<Position>::Builder builder =
+ superstructure_position_sender_.MakeBuilder();
- pot_encoder_intake_.GetSensorValues(&position->intake);
- pot_encoder_shoulder_.GetSensorValues(&position->shoulder);
- pot_encoder_wrist_.GetSensorValues(&position->wrist);
+ frc971::PotAndIndexPosition::Builder intake_builder =
+ builder.MakeBuilder<frc971::PotAndIndexPosition>();
- position.Send();
+ flatbuffers::Offset<frc971::PotAndIndexPosition> intake_offset =
+ pot_encoder_intake_.GetSensorValues(&intake_builder);
+
+ frc971::PotAndIndexPosition::Builder shoulder_builder =
+ builder.MakeBuilder<frc971::PotAndIndexPosition>();
+ flatbuffers::Offset<frc971::PotAndIndexPosition> shoulder_offset =
+ pot_encoder_shoulder_.GetSensorValues(&shoulder_builder);
+
+ frc971::PotAndIndexPosition::Builder wrist_builder =
+ builder.MakeBuilder<frc971::PotAndIndexPosition>();
+ flatbuffers::Offset<frc971::PotAndIndexPosition> wrist_offset =
+ pot_encoder_wrist_.GetSensorValues(&wrist_builder);
+
+ Position::Builder position_builder = builder.MakeBuilder<Position>();
+
+ position_builder.add_intake(intake_offset);
+ position_builder.add_shoulder(shoulder_offset);
+ position_builder.add_wrist(wrist_offset);
+
+ builder.Send(position_builder.Finish());
}
double shoulder_angle() const { return arm_plant_->X(0, 0); }
@@ -180,37 +197,39 @@
// Feed voltages into physics simulation.
::Eigen::Matrix<double, 1, 1> intake_U;
::Eigen::Matrix<double, 2, 1> arm_U;
- intake_U << superstructure_output_fetcher_->voltage_intake +
+ intake_U << superstructure_output_fetcher_->voltage_intake() +
intake_plant_->voltage_offset();
- arm_U << superstructure_output_fetcher_->voltage_shoulder +
+ arm_U << superstructure_output_fetcher_->voltage_shoulder() +
arm_plant_->shoulder_voltage_offset(),
- superstructure_output_fetcher_->voltage_wrist +
+ superstructure_output_fetcher_->voltage_wrist() +
arm_plant_->wrist_voltage_offset();
// Verify that the correct power limits are being respected depending on
// which mode we are in.
EXPECT_TRUE(superstructure_status_fetcher_.Fetch());
- if (superstructure_status_fetcher_->state == Superstructure::RUNNING ||
- superstructure_status_fetcher_->state ==
+ if (superstructure_status_fetcher_->state() == Superstructure::RUNNING ||
+ superstructure_status_fetcher_->state() ==
Superstructure::LANDING_RUNNING) {
- AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_intake),
+ AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_intake()),
Superstructure::kOperatingVoltage);
- AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_shoulder),
- Superstructure::kOperatingVoltage);
- AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_wrist),
+ AOS_CHECK_LE(
+ ::std::abs(superstructure_output_fetcher_->voltage_shoulder()),
+ Superstructure::kOperatingVoltage);
+ AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_wrist()),
Superstructure::kOperatingVoltage);
} else {
- AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_intake),
+ AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_intake()),
Superstructure::kZeroingVoltage);
- AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_shoulder),
- Superstructure::kZeroingVoltage);
- AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_wrist),
+ AOS_CHECK_LE(
+ ::std::abs(superstructure_output_fetcher_->voltage_shoulder()),
+ Superstructure::kZeroingVoltage);
+ AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_wrist()),
Superstructure::kZeroingVoltage);
}
if (arm_plant_->X(0, 0) <=
Superstructure::kShoulderTransitionToLanded + 1e-4) {
- AOS_CHECK_GE(superstructure_output_fetcher_->voltage_shoulder,
+ AOS_CHECK_GE(superstructure_output_fetcher_->voltage_shoulder(),
Superstructure::kLandingShoulderDownVoltage - 0.00001);
}
@@ -309,9 +328,9 @@
bool first_ = true;
- ::aos::Sender<SuperstructureQueue::Position> superstructure_position_sender_;
- ::aos::Fetcher<SuperstructureQueue::Status> superstructure_status_fetcher_;
- ::aos::Fetcher<SuperstructureQueue::Output> superstructure_output_fetcher_;
+ ::aos::Sender<Position> superstructure_position_sender_;
+ ::aos::Fetcher<Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<Output> superstructure_output_fetcher_;
::std::unique_ptr<IntakePlant> intake_plant_;
::std::unique_ptr<ArmPlant> arm_plant_;
@@ -334,17 +353,16 @@
class SuperstructureTest : public ::aos::testing::ControlLoopTest {
protected:
SuperstructureTest()
- : ::aos::testing::ControlLoopTest(chrono::microseconds(5000)),
+ : ::aos::testing::ControlLoopTest(
+ aos::configuration::ReadConfig("y2016/config.json"),
+ chrono::microseconds(5000)),
test_event_loop_(MakeEventLoop()),
superstructure_goal_fetcher_(
- test_event_loop_->MakeFetcher<SuperstructureQueue::Goal>(
- ".y2016.control_loops.superstructure_queue.goal")),
+ test_event_loop_->MakeFetcher<Goal>("/superstructure")),
superstructure_goal_sender_(
- test_event_loop_->MakeSender<SuperstructureQueue::Goal>(
- ".y2016.control_loops.superstructure_queue.goal")),
+ test_event_loop_->MakeSender<Goal>("/superstructure")),
superstructure_status_fetcher_(
- test_event_loop_->MakeFetcher<SuperstructureQueue::Status>(
- ".y2016.control_loops.superstructure_queue.status")),
+ test_event_loop_->MakeFetcher<Status>("/superstructure")),
superstructure_event_loop_(MakeEventLoop()),
superstructure_(superstructure_event_loop_.get()),
superstructure_plant_event_loop_(MakeEventLoop()),
@@ -357,26 +375,26 @@
EXPECT_TRUE(superstructure_goal_fetcher_.get() != nullptr);
EXPECT_TRUE(superstructure_status_fetcher_.get() != nullptr);
- EXPECT_NEAR(superstructure_goal_fetcher_->angle_intake,
- superstructure_status_fetcher_->intake.angle, 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->angle_shoulder,
- superstructure_status_fetcher_->shoulder.angle, 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->angle_wrist,
- superstructure_status_fetcher_->wrist.angle, 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_intake(),
+ superstructure_status_fetcher_->intake()->angle(), 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_shoulder(),
+ superstructure_status_fetcher_->shoulder()->angle(), 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_wrist(),
+ superstructure_status_fetcher_->wrist()->angle(), 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->angle_intake,
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_intake(),
superstructure_plant_.intake_angle(), 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->angle_shoulder,
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_shoulder(),
superstructure_plant_.shoulder_angle(), 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->angle_wrist,
+ EXPECT_NEAR(superstructure_goal_fetcher_->angle_wrist(),
superstructure_plant_.wrist_angle(), 0.001);
}
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
- ::aos::Fetcher<SuperstructureQueue::Goal> superstructure_goal_fetcher_;
- ::aos::Sender<SuperstructureQueue::Goal> superstructure_goal_sender_;
- ::aos::Fetcher<SuperstructureQueue::Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<Goal> superstructure_goal_fetcher_;
+ ::aos::Sender<Goal> superstructure_goal_sender_;
+ ::aos::Fetcher<Status> superstructure_status_fetcher_;
// Create a control loop and simulation.
::std::unique_ptr<::aos::EventLoop> superstructure_event_loop_;
@@ -390,17 +408,18 @@
TEST_F(SuperstructureTest, DoesNothing) {
SetEnabled(true);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0;
- message->angle_shoulder = 0;
- message->angle_wrist = 0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0);
+ goal_builder.add_angle_shoulder(0);
+ goal_builder.add_angle_wrist(0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
@@ -412,17 +431,18 @@
SetEnabled(true);
// Set a reasonable goal.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = M_PI / 4.0;
- message->angle_shoulder = 1.4;
- message->angle_wrist = M_PI / 4.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(M_PI / 4.0);
+ goal_builder.add_angle_shoulder(1.4);
+ goal_builder.add_angle_wrist(M_PI / 4.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Give it a lot of time to get there.
@@ -437,43 +457,45 @@
SetEnabled(true);
// Set some ridiculous goals to test upper limits.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = M_PI * 10;
- message->angle_shoulder = M_PI * 10;
- message->angle_wrist = M_PI * 10;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(M_PI * 10);
+ goal_builder.add_angle_shoulder(M_PI * 10);
+ goal_builder.add_angle_wrist(M_PI * 10);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
// Check that we are near our soft limit.
superstructure_status_fetcher_.Fetch();
EXPECT_NEAR(constants::Values::kIntakeRange.upper,
- superstructure_status_fetcher_->intake.angle, 0.001);
+ superstructure_status_fetcher_->intake()->angle(), 0.001);
EXPECT_NEAR(constants::Values::kShoulderRange.upper,
- superstructure_status_fetcher_->shoulder.angle, 0.001);
+ superstructure_status_fetcher_->shoulder()->angle(), 0.001);
EXPECT_NEAR(constants::Values::kWristRange.upper +
constants::Values::kShoulderRange.upper,
- superstructure_status_fetcher_->wrist.angle, 0.001);
+ superstructure_status_fetcher_->wrist()->angle(), 0.001);
// Set some ridiculous goals to test limits.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = M_PI * 10;
- message->angle_shoulder = M_PI * 10;
- message->angle_wrist = -M_PI * 10.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(M_PI * 10);
+ goal_builder.add_angle_shoulder(M_PI * 10);
+ goal_builder.add_angle_wrist(-M_PI * 10.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -481,26 +503,27 @@
// Check that we are near our soft limit.
superstructure_status_fetcher_.Fetch();
EXPECT_NEAR(constants::Values::kIntakeRange.upper,
- superstructure_status_fetcher_->intake.angle, 0.001);
+ superstructure_status_fetcher_->intake()->angle(), 0.001);
EXPECT_NEAR(constants::Values::kShoulderRange.upper,
- superstructure_status_fetcher_->shoulder.angle, 0.001);
+ superstructure_status_fetcher_->shoulder()->angle(), 0.001);
EXPECT_NEAR(constants::Values::kWristRange.lower +
constants::Values::kShoulderRange.upper,
- superstructure_status_fetcher_->wrist.angle, 0.001);
+ superstructure_status_fetcher_->wrist()->angle(), 0.001);
// Set some ridiculous goals to test lower limits.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = -M_PI * 10;
- message->angle_shoulder = -M_PI * 10;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(-M_PI * 10);
+ goal_builder.add_angle_shoulder(-M_PI * 10);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -508,27 +531,28 @@
// Check that we are near our soft limit.
superstructure_status_fetcher_.Fetch();
EXPECT_NEAR(constants::Values::kIntakeRange.lower,
- superstructure_status_fetcher_->intake.angle, 0.001);
+ superstructure_status_fetcher_->intake()->angle(), 0.001);
EXPECT_NEAR(constants::Values::kShoulderRange.lower,
- superstructure_status_fetcher_->shoulder.angle, 0.001);
- EXPECT_NEAR(0.0, superstructure_status_fetcher_->wrist.angle, 0.001);
+ superstructure_status_fetcher_->shoulder()->angle(), 0.001);
+ EXPECT_NEAR(0.0, superstructure_status_fetcher_->wrist()->angle(), 0.001);
}
// Tests that the loop zeroes when run for a while.
TEST_F(SuperstructureTest, ZeroTest) {
SetEnabled(true);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.lower;
- message->angle_shoulder = constants::Values::kShoulderRange.lower;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(constants::Values::kIntakeRange.lower);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.lower);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -557,12 +581,13 @@
superstructure_plant_.InitializeRelativeWristPosition(
constants::Values::kWristRange.lower);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.upper;
- message->angle_shoulder = constants::Values::kShoulderRange.upper;
- message->angle_wrist = constants::Values::kWristRange.upper +
- constants::Values::kShoulderRange.upper;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(constants::Values::kIntakeRange.upper);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.upper);
+ goal_builder.add_angle_wrist(constants::Values::kWristRange.upper +
+ constants::Values::kShoulderRange.upper);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// We have to wait for it to put the elevator in a safe position as well.
RunFor(chrono::seconds(15));
@@ -580,11 +605,12 @@
superstructure_plant_.InitializeRelativeWristPosition(
constants::Values::kWristRange.upper);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.lower;
- message->angle_shoulder = constants::Values::kShoulderRange.lower;
- message->angle_wrist = 0.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(constants::Values::kIntakeRange.lower);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.lower);
+ goal_builder.add_angle_wrist(0.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// We have to wait for it to put the superstructure in a safe position as
// well.
@@ -604,11 +630,12 @@
constants::Values::kWristRange.upper);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.lower + 0.3;
- message->angle_shoulder = constants::Values::kShoulderRange.upper;
- message->angle_wrist = 0.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(constants::Values::kIntakeRange.lower + 0.3);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.upper);
+ goal_builder.add_angle_wrist(0.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(15));
@@ -628,11 +655,13 @@
superstructure_plant_.set_check_for_collisions(false);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.lower + 0.03;
- message->angle_shoulder = constants::Values::kShoulderRange.lower + 0.03;
- message->angle_wrist = constants::Values::kWristRange.lower + 0.03;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(constants::Values::kIntakeRange.lower + 0.03);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.lower +
+ 0.03);
+ goal_builder.add_angle_wrist(constants::Values::kWristRange.lower + 0.03);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(100));
@@ -659,17 +688,18 @@
constants::Values::kWristRange.upper);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.upper;
- message->angle_shoulder = constants::Values::kShoulderRange.upper;
- message->angle_wrist = constants::Values::kWristRange.upper;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(constants::Values::kIntakeRange.upper);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.upper);
+ goal_builder.add_angle_wrist(constants::Values::kWristRange.upper);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Expected states to cycle through and check in order.
@@ -730,17 +760,18 @@
superstructure_plant_.InitializeAbsoluteWristPosition(0.0);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.lower;
- message->angle_shoulder = constants::Values::kShoulderRange.lower;
- message->angle_wrist = constants::Values::kWristRange.lower;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(constants::Values::kIntakeRange.lower);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.lower);
+ goal_builder.add_angle_wrist(constants::Values::kWristRange.lower);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Expected states to cycle through and check in order.
@@ -812,11 +843,12 @@
superstructure_plant_.InitializeRelativeWristPosition(0.0);
superstructure_plant_.set_power_error(1.0, 1.0, 1.0);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 0.0;
- message->angle_wrist = 0.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(0.0);
+ goal_builder.add_angle_wrist(0.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(8));
@@ -834,12 +866,13 @@
superstructure_plant_.InitializeRelativeWristPosition(-0.001);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder =
- constants::Values::kShoulderEncoderIndexDifference * 10;
- message->angle_wrist = 0.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(
+ constants::Values::kShoulderEncoderIndexDifference * 10);
+ goal_builder.add_angle_wrist(0.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Run disabled for 2 seconds
@@ -883,17 +916,18 @@
TEST_F(SuperstructureTest, ShoulderAccelerationLimitTest) {
SetEnabled(true);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 1.0;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(1.0);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -902,17 +936,18 @@
VerifyNearGoal();
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 1.5;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 1;
- message->max_angular_acceleration_intake = 1;
- message->max_angular_velocity_shoulder = 1;
- message->max_angular_acceleration_shoulder = 1;
- message->max_angular_velocity_wrist = 1;
- message->max_angular_acceleration_wrist = 1;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(1.5);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(1);
+ goal_builder.add_max_angular_acceleration_intake(1);
+ goal_builder.add_max_angular_velocity_shoulder(1);
+ goal_builder.add_max_angular_acceleration_shoulder(1);
+ goal_builder.add_max_angular_velocity_wrist(1);
+ goal_builder.add_max_angular_acceleration_wrist(1);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// TODO(austin): The profile isn't feasible, so when we try to track it, we
@@ -930,17 +965,18 @@
TEST_F(SuperstructureTest, IntakeAccelerationLimitTest) {
SetEnabled(true);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 1.0;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(1.0);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -949,17 +985,18 @@
VerifyNearGoal();
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.5;
- message->angle_shoulder = 1.0;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 1;
- message->max_angular_acceleration_intake = 1;
- message->max_angular_velocity_shoulder = 1;
- message->max_angular_acceleration_shoulder = 1;
- message->max_angular_velocity_wrist = 1;
- message->max_angular_acceleration_wrist = 1;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.5);
+ goal_builder.add_angle_shoulder(1.0);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(1);
+ goal_builder.add_max_angular_acceleration_intake(1);
+ goal_builder.add_max_angular_velocity_shoulder(1);
+ goal_builder.add_max_angular_acceleration_shoulder(1);
+ goal_builder.add_max_angular_velocity_wrist(1);
+ goal_builder.add_max_angular_acceleration_wrist(1);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
superstructure_plant_.set_peak_intake_acceleration(1.20);
@@ -974,18 +1011,19 @@
TEST_F(SuperstructureTest, WristAccelerationLimitTest) {
SetEnabled(true);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder =
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -994,18 +1032,19 @@
VerifyNearGoal();
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder =
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1;
- message->angle_wrist = 0.5;
- message->max_angular_velocity_intake = 1;
- message->max_angular_acceleration_intake = 1;
- message->max_angular_velocity_shoulder = 1;
- message->max_angular_acceleration_shoulder = 1;
- message->max_angular_velocity_wrist = 1;
- message->max_angular_acceleration_wrist = 1;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1);
+ goal_builder.add_angle_wrist(0.5);
+ goal_builder.add_max_angular_velocity_intake(1);
+ goal_builder.add_max_angular_acceleration_intake(1);
+ goal_builder.add_max_angular_velocity_shoulder(1);
+ goal_builder.add_max_angular_acceleration_shoulder(1);
+ goal_builder.add_max_angular_velocity_wrist(1);
+ goal_builder.add_max_angular_acceleration_wrist(1);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
superstructure_plant_.set_peak_intake_acceleration(1.05);
@@ -1021,18 +1060,19 @@
TEST_F(SuperstructureTest, SaturatedIntakeProfileTest) {
SetEnabled(true);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder =
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -1041,18 +1081,19 @@
VerifyNearGoal();
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.5;
- message->angle_shoulder =
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 4.5;
- message->max_angular_acceleration_intake = 800;
- message->max_angular_velocity_shoulder = 1;
- message->max_angular_acceleration_shoulder = 100;
- message->max_angular_velocity_wrist = 1;
- message->max_angular_acceleration_wrist = 100;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.5);
+ goal_builder.add_angle_shoulder(
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(4.5);
+ goal_builder.add_max_angular_acceleration_intake(800);
+ goal_builder.add_max_angular_velocity_shoulder(1);
+ goal_builder.add_max_angular_acceleration_shoulder(100);
+ goal_builder.add_max_angular_velocity_wrist(1);
+ goal_builder.add_max_angular_acceleration_wrist(100);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
superstructure_plant_.set_peak_intake_velocity(4.65);
@@ -1068,17 +1109,18 @@
TEST_F(SuperstructureTest, SaturatedShoulderProfileTest) {
SetEnabled(true);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 1.0;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(1.0);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -1087,17 +1129,18 @@
VerifyNearGoal();
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 1.9;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 1.0;
- message->max_angular_acceleration_intake = 1.0;
- message->max_angular_velocity_shoulder = 5.0;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 1;
- message->max_angular_acceleration_wrist = 100;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(1.9);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(1.0);
+ goal_builder.add_max_angular_acceleration_intake(1.0);
+ goal_builder.add_max_angular_velocity_shoulder(5.0);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(1);
+ goal_builder.add_max_angular_acceleration_wrist(100);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
superstructure_plant_.set_peak_intake_velocity(1.0);
@@ -1113,18 +1156,19 @@
TEST_F(SuperstructureTest, SaturatedWristProfileTest) {
SetEnabled(true);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder =
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -1133,18 +1177,19 @@
VerifyNearGoal();
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder =
- CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1;
- message->angle_wrist = 1.3;
- message->max_angular_velocity_intake = 1.0;
- message->max_angular_acceleration_intake = 1.0;
- message->max_angular_velocity_shoulder = 1.0;
- message->max_angular_acceleration_shoulder = 1.0;
- message->max_angular_velocity_wrist = 10.0;
- message->max_angular_acceleration_wrist = 160.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(
+ CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference + 0.1);
+ goal_builder.add_angle_wrist(1.3);
+ goal_builder.add_max_angular_velocity_intake(1.0);
+ goal_builder.add_max_angular_acceleration_intake(1.0);
+ goal_builder.add_max_angular_velocity_shoulder(1.0);
+ goal_builder.add_max_angular_acceleration_shoulder(1.0);
+ goal_builder.add_max_angular_velocity_wrist(10.0);
+ goal_builder.add_max_angular_acceleration_wrist(160.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
superstructure_plant_.set_peak_intake_velocity(1.0);
@@ -1165,21 +1210,26 @@
superstructure_plant_.InitializeAbsoluteWristPosition(0.0);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.upper; // stowed
- message->angle_shoulder = constants::Values::kShoulderRange.lower; // Down
- message->angle_wrist = 0.0; // Stowed
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(
+ constants::Values::kIntakeRange.upper); // stowed
+ goal_builder.add_angle_shoulder(
+ constants::Values::kShoulderRange.lower); // Down
+ goal_builder.add_angle_wrist(0.0); // Stowed
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(15));
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.upper; // stowed
- message->angle_shoulder = M_PI / 4.0; // in the collision area
- message->angle_wrist = M_PI / 2.0; // down
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(
+ constants::Values::kIntakeRange.upper); // stowed
+ goal_builder.add_angle_shoulder(M_PI / 4.0); // in the collision area
+ goal_builder.add_angle_wrist(M_PI / 2.0); // down
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
@@ -1188,27 +1238,30 @@
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
// The intake should be out of the way despite being told to move to stowing.
- EXPECT_LT(superstructure_status_fetcher_->intake.angle, M_PI);
- EXPECT_LT(superstructure_status_fetcher_->intake.angle,
+ EXPECT_LT(superstructure_status_fetcher_->intake()->angle(), M_PI);
+ EXPECT_LT(superstructure_status_fetcher_->intake()->angle(),
constants::Values::kIntakeRange.upper);
- EXPECT_LT(superstructure_status_fetcher_->intake.angle,
+ EXPECT_LT(superstructure_status_fetcher_->intake()->angle(),
CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference);
// The arm should have reached its goal.
- EXPECT_NEAR(M_PI / 4.0, superstructure_status_fetcher_->shoulder.angle, 0.001);
+ EXPECT_NEAR(M_PI / 4.0, superstructure_status_fetcher_->shoulder()->angle(),
+ 0.001);
// The wrist should be forced into a stowing position.
// Since the intake is kicked out, we can be within
// kMaxWristAngleForMovingByIntake
- EXPECT_NEAR(0, superstructure_status_fetcher_->wrist.angle,
+ EXPECT_NEAR(0, superstructure_status_fetcher_->wrist()->angle(),
CollisionAvoidance::kMaxWristAngleForMovingByIntake + 0.001);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = constants::Values::kIntakeRange.upper; // stowed
- message->angle_shoulder = M_PI / 2.0; // in the collision area
- message->angle_wrist = M_PI; // forward
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(
+ constants::Values::kIntakeRange.upper); // stowed
+ goal_builder.add_angle_shoulder(M_PI / 2.0); // in the collision area
+ goal_builder.add_angle_wrist(M_PI); // forward
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
@@ -1224,11 +1277,12 @@
superstructure_plant_.InitializeAbsoluteWristPosition(M_PI); // forward
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 0.0;
- message->angle_wrist = M_PI; // intentionally asking for forward
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(0.0);
+ goal_builder.add_angle_wrist(M_PI); // intentionally asking for forward
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(15));
@@ -1237,12 +1291,12 @@
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
// The intake should be in intaking position, as asked.
- EXPECT_NEAR(0.0, superstructure_status_fetcher_->intake.angle, 0.001);
+ EXPECT_NEAR(0.0, superstructure_status_fetcher_->intake()->angle(), 0.001);
// The shoulder and wrist should both be at zero degrees (i.e.
// stowed/intaking position).
- EXPECT_NEAR(0.0, superstructure_status_fetcher_->shoulder.angle, 0.001);
- EXPECT_NEAR(0.0, superstructure_status_fetcher_->wrist.angle, 0.001);
+ EXPECT_NEAR(0.0, superstructure_status_fetcher_->shoulder()->angle(), 0.001);
+ EXPECT_NEAR(0.0, superstructure_status_fetcher_->wrist()->angle(), 0.001);
}
// Make sure that we can properly detect a collision.
@@ -1250,11 +1304,12 @@
SetEnabled(true);
// Zero & go straight up with the shoulder.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = M_PI * 0.5;
- message->angle_wrist = 0.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(M_PI * 0.5);
+ goal_builder.add_angle_wrist(0.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -1290,11 +1345,12 @@
SetEnabled(true);
// Zero & go straight up with the shoulder.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = constants::Values::kShoulderRange.lower;
- message->angle_wrist = 0.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.lower);
+ goal_builder.add_angle_wrist(0.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -1333,11 +1389,12 @@
superstructure_plant_.InitializeAbsoluteWristPosition(0.0);
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = constants::Values::kShoulderRange.lower;
- message->angle_wrist = 0.0; // intentionally asking for forward
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(constants::Values::kShoulderRange.lower);
+ goal_builder.add_angle_wrist(0.0); // intentionally asking for forward
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(6));
@@ -1353,7 +1410,7 @@
RunFor(chrono::seconds(2));
superstructure_goal_fetcher_.Fetch();
EXPECT_LE(constants::Values::kShoulderRange.lower,
- superstructure_goal_fetcher_->angle_shoulder);
+ superstructure_goal_fetcher_->angle_shoulder());
}
// Make sure that we land slowly.
@@ -1361,27 +1418,29 @@
SetEnabled(true);
// Zero & go to initial position.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = M_PI * 0.25;
- message->angle_wrist = 0.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(M_PI * 0.25);
+ goal_builder.add_angle_wrist(0.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(8));
// Tell it to land in the bellypan as fast as possible.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 0.0;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(0.0);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Wait until we hit the transition point.
@@ -1400,27 +1459,29 @@
SetEnabled(true);
// Zero & go to initial position.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = 0.0;
- message->angle_wrist = 0.0;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(0.0);
+ goal_builder.add_angle_wrist(0.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(8));
// Tell it to take off as fast as possible.
{
- auto message = superstructure_goal_sender_.MakeMessage();
- message->angle_intake = 0.0;
- message->angle_shoulder = M_PI / 2.0;
- message->angle_wrist = 0.0;
- message->max_angular_velocity_intake = 20;
- message->max_angular_acceleration_intake = 20;
- message->max_angular_velocity_shoulder = 20;
- message->max_angular_acceleration_shoulder = 20;
- message->max_angular_velocity_wrist = 20;
- message->max_angular_acceleration_wrist = 20;
- ASSERT_TRUE(message.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_angle_intake(0.0);
+ goal_builder.add_angle_shoulder(M_PI / 2.0);
+ goal_builder.add_angle_wrist(0.0);
+ goal_builder.add_max_angular_velocity_intake(20);
+ goal_builder.add_max_angular_acceleration_intake(20);
+ goal_builder.add_max_angular_velocity_shoulder(20);
+ goal_builder.add_max_angular_acceleration_shoulder(20);
+ goal_builder.add_max_angular_velocity_wrist(20);
+ goal_builder.add_max_angular_acceleration_wrist(20);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Wait until we hit the transition point.
diff --git a/y2016/control_loops/superstructure/superstructure_main.cc b/y2016/control_loops/superstructure/superstructure_main.cc
index 00fa7dd..abe2ec9 100644
--- a/y2016/control_loops/superstructure/superstructure_main.cc
+++ b/y2016/control_loops/superstructure/superstructure_main.cc
@@ -1,12 +1,15 @@
#include "y2016/control_loops/superstructure/superstructure.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
int main() {
::aos::InitNRT(true);
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::y2016::control_loops::superstructure::Superstructure superstructure(
&event_loop);
diff --git a/y2016/control_loops/superstructure/superstructure_output.fbs b/y2016/control_loops/superstructure/superstructure_output.fbs
new file mode 100644
index 0000000..40a0091
--- /dev/null
+++ b/y2016/control_loops/superstructure/superstructure_output.fbs
@@ -0,0 +1,22 @@
+namespace y2016.control_loops.superstructure;
+
+table Output {
+ voltage_intake:float;
+ voltage_shoulder:float;
+ voltage_wrist:float;
+
+ voltage_top_rollers:float;
+ voltage_bottom_rollers:float;
+
+ // Voltage to sent to the climber. Positive is pulling the robot up.
+ voltage_climber:float;
+ // If true, release the latch to trigger the climber to unfold.
+ unfold_climber:bool;
+
+ // If true, release the latch to hold the traverse mechanism in the middle.
+ traverse_unlatched:bool;
+ // If true, fire the traverse mechanism down.
+ traverse_down:bool;
+}
+
+root_type Output;
diff --git a/y2016/control_loops/superstructure/superstructure_position.fbs b/y2016/control_loops/superstructure/superstructure_position.fbs
new file mode 100644
index 0000000..fb356e0
--- /dev/null
+++ b/y2016/control_loops/superstructure/superstructure_position.fbs
@@ -0,0 +1,19 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2016.control_loops.superstructure;
+
+table Position {
+ // Zero for the intake potentiometer value is horizontal, and positive is
+ // up.
+ // Zero for the shoulder potentiometer value is horizontal, and positive is
+ // up.
+ // Zero for the wrist potentiometer value is parallel to the arm and with
+ // the shooter wheels pointed towards the shoulder joint. This is measured
+ // relative to the arm, not the ground, not like the world we actually
+ // present to users.
+ intake:frc971.PotAndIndexPosition;
+ shoulder:frc971.PotAndIndexPosition;
+ wrist:frc971.PotAndIndexPosition;
+}
+
+root_type Position;
diff --git a/y2016/control_loops/superstructure/superstructure_status.fbs b/y2016/control_loops/superstructure/superstructure_status.fbs
new file mode 100644
index 0000000..373bfe2
--- /dev/null
+++ b/y2016/control_loops/superstructure/superstructure_status.fbs
@@ -0,0 +1,56 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2016.control_loops.superstructure;
+
+table JointState {
+ // Angle of the joint in radians.
+ angle:float;
+ // Angular velocity of the joint in radians/second.
+ angular_velocity:float;
+ // Profiled goal angle of the joint in radians.
+ goal_angle:float;
+ // Profiled goal angular velocity of the joint in radians/second.
+ goal_angular_velocity:float;
+ // Unprofiled goal angle of the joint in radians.
+ unprofiled_goal_angle:float;
+ // Unprofiled goal angular velocity of the joint in radians/second.
+ unprofiled_goal_angular_velocity:float;
+
+ // The estimated voltage error.
+ voltage_error:float;
+
+ // The calculated velocity with delta x/delta t
+ calculated_velocity:float;
+
+ // Components of the control loop output
+ position_power:float;
+ velocity_power:float;
+ feedforwards_power:float;
+
+ // State of the estimator.
+ estimator_state:frc971.EstimatorState;
+}
+
+table Status {
+ // Are the superstructure subsystems zeroed?
+ zeroed:bool;
+
+ // If true, we have aborted.
+ estopped:bool;
+
+ // The internal state of the state machine.
+ state:int;
+
+
+ // Estimated angles and angular velocities of the superstructure subsystems.
+ intake:JointState;
+ shoulder:JointState;
+ wrist:JointState;
+
+ shoulder_controller_index:int;
+
+ // Is the superstructure collided?
+ is_collided:bool;
+}
+
+root_type Status;
diff --git a/y2016/dashboard/BUILD b/y2016/dashboard/BUILD
index f3998a1..dbc6214 100644
--- a/y2016/dashboard/BUILD
+++ b/y2016/dashboard/BUILD
@@ -1,5 +1,5 @@
load("//aos/seasocks:gen_embedded.bzl", "gen_embedded")
-load("//aos/downloader:downloader.bzl", "aos_downloader_dir")
+load("//frc971/downloader:downloader.bzl", "aos_downloader_dir")
gen_embedded(
name = "gen_embedded",
@@ -28,16 +28,17 @@
deps = [
":gen_embedded",
"//aos:init",
- "//aos/events:event-loop",
- "//aos/events:shm-event-loop",
+ "//aos/events:event_loop",
+ "//aos/events:shm_event_loop",
"//aos/logging",
"//aos/seasocks:seasocks_logger",
"//aos/time",
"//aos/util:phased_loop",
- "//frc971/autonomous:auto_queue",
+ "//frc971/autonomous:auto_fbs",
+ "//frc971/control_loops:control_loops_fbs",
"//third_party/seasocks",
- "//y2016/control_loops/superstructure:superstructure_queue",
- "//y2016/queues:ball_detector",
- "//y2016/vision:vision_queue",
+ "//y2016/control_loops/superstructure:superstructure_status_fbs",
+ "//y2016/queues:ball_detector_fbs",
+ "//y2016/vision:vision_fbs",
],
)
diff --git a/y2016/dashboard/dashboard.cc b/y2016/dashboard/dashboard.cc
index 27036e0..7f13fbe 100644
--- a/y2016/dashboard/dashboard.cc
+++ b/y2016/dashboard/dashboard.cc
@@ -11,17 +11,18 @@
#include "internal/Embedded.h"
#include "seasocks/Server.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
#include "aos/mutex/mutex.h"
+#include "aos/realtime.h"
#include "aos/seasocks/seasocks_logger.h"
#include "aos/time/time.h"
#include "aos/util/phased_loop.h"
-#include "frc971/autonomous/auto.q.h"
-#include "y2016/control_loops/superstructure/superstructure.q.h"
-#include "y2016/queues/ball_detector.q.h"
-#include "y2016/vision/vision.q.h"
+#include "frc971/autonomous/auto_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 chrono = ::std::chrono;
@@ -52,17 +53,17 @@
: event_loop_(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")),
autonomous_mode_fetcher_(
event_loop->MakeFetcher<::frc971::autonomous::AutonomousMode>(
- ".frc971.autonomous.auto_mode")),
+ "/aos")),
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")),
cur_raw_data_("no data"),
sample_id_(0),
measure_index_(0),
@@ -121,22 +122,22 @@
// TODO(comran): Grab detected voltage from joystick_reader. Except this
// value may not change, so it may be worth it to keep it as it is right
// now.
- if (ball_detector_fetcher_->voltage > 2.5) {
+ if (ball_detector_fetcher_->voltage() > 2.5) {
big_indicator = big_indicator::kBallIntaked;
}
}
if (superstructure_status_fetcher_.get()) {
- if (!superstructure_status_fetcher_->zeroed) {
+ if (!superstructure_status_fetcher_->zeroed()) {
superstructure_state_indicator = superstructure_indicator::kNotZeroed;
}
- if (superstructure_status_fetcher_->estopped) {
+ if (superstructure_status_fetcher_->estopped()) {
superstructure_state_indicator = superstructure_indicator::kEstopped;
}
}
if (autonomous_mode_fetcher_.get()) {
- auto_mode_indicator = autonomous_mode_fetcher_->mode;
+ auto_mode_indicator = autonomous_mode_fetcher_->mode();
}
AddPoint("big indicator", big_indicator);
@@ -285,7 +286,10 @@
::aos::InitNRT();
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::seasocks::Server server(::std::shared_ptr<seasocks::Logger>(
new ::aos::seasocks::SeasocksLogger(::seasocks::Logger::Level::Info)));
diff --git a/y2016/dashboard/dashboard.h b/y2016/dashboard/dashboard.h
index ed5a579..f019ec7 100644
--- a/y2016/dashboard/dashboard.h
+++ b/y2016/dashboard/dashboard.h
@@ -14,13 +14,13 @@
#include "seasocks/StringUtil.h"
#include "seasocks/WebSocket.h"
-#include "aos/events/event-loop.h"
+#include "aos/events/event_loop.h"
#include "aos/mutex/mutex.h"
#include "aos/time/time.h"
-#include "frc971/autonomous/auto.q.h"
-#include "y2016/control_loops/superstructure/superstructure.q.h"
-#include "y2016/queues/ball_detector.q.h"
-#include "y2016/vision/vision.q.h"
+#include "frc971/autonomous/auto_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 dashboard {
@@ -71,7 +71,7 @@
::aos::Fetcher<::y2016::vision::VisionStatus> vision_status_fetcher_;
::aos::Fetcher<::y2016::sensors::BallDetector> ball_detector_fetcher_;
::aos::Fetcher<::frc971::autonomous::AutonomousMode> autonomous_mode_fetcher_;
- ::aos::Fetcher<::y2016::control_loops::SuperstructureQueue::Status>
+ ::aos::Fetcher<::y2016::control_loops::superstructure::Status>
superstructure_status_fetcher_;
// Storage vector that is written and overwritten with data in a FIFO fashion.
diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index 276fda6..50f2f8a 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -11,19 +11,20 @@
#include "aos/time/time.h"
#include "aos/util/log_interval.h"
-#include "frc971/autonomous/auto.q.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/queues/gyro.q.h"
+#include "frc971/autonomous/auto_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "y2016/actors/autonomous_actor.h"
#include "y2016/actors/superstructure_actor.h"
#include "y2016/actors/vision_align_actor.h"
#include "y2016/constants.h"
#include "y2016/control_loops/drivetrain/drivetrain_base.h"
-#include "y2016/control_loops/shooter/shooter.q.h"
+#include "y2016/control_loops/shooter/shooter_goal_generated.h"
#include "y2016/control_loops/superstructure/superstructure.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/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"
using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::ControlBit;
@@ -73,30 +74,28 @@
::aos::input::DrivetrainInputReader::InputType::kSteeringWheel, {}),
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")),
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")),
superstructure_goal_sender_(
event_loop
- ->MakeSender<::y2016::control_loops::SuperstructureQueue::Goal>(
- ".y2016.control_loops.superstructure_queue.goal")),
+ ->MakeSender<::y2016::control_loops::superstructure::Goal>(
+ "/superstructure")),
drivetrain_goal_fetcher_(
- event_loop
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Goal>(
- ".frc971.control_loops.drivetrain_queue.goal")),
+ event_loop->MakeFetcher<::frc971::control_loops::drivetrain::Goal>(
+ "/drivetrain")),
drivetrain_status_fetcher_(
event_loop
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
- ".frc971.control_loops.drivetrain_queue.status")),
+ ->MakeFetcher<::frc971::control_loops::drivetrain::Status>(
+ "/drivetrain")),
intake_goal_(0.0),
shoulder_goal_(M_PI / 2.0),
wrist_goal_(0.0),
@@ -116,14 +115,14 @@
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();
}
if (data.IsPressed(kVisionAlign)) {
if (vision_valid_ && !vision_action_running_) {
- actors::VisionAlignActionParams params;
+ actors::vision_align_action::VisionAlignActionParamsT params;
EnqueueAction(vision_align_action_factory_.Make(params));
vision_action_running_ = true;
AOS_LOG(INFO, "Starting vision align\n");
@@ -167,7 +166,7 @@
}
if (superstructure_status_fetcher_.get() &&
- superstructure_status_fetcher_->zeroed) {
+ superstructure_status_fetcher_->zeroed()) {
if (waiting_for_zero_) {
AOS_LOG(DEBUG, "Zeroed! Starting teleop mode.\n");
waiting_for_zero_ = false;
@@ -191,7 +190,7 @@
wrist_goal_ = M_PI + 0.41 + 0.02 - 0.005;
drivetrain_status_fetcher_.Fetch();
if (drivetrain_status_fetcher_.get()) {
- wrist_goal_ += drivetrain_status_fetcher_->ground_angle;
+ wrist_goal_ += drivetrain_status_fetcher_->ground_angle();
}
shooter_velocity_ = 640.0;
intake_goal_ = intake_when_shooting;
@@ -201,7 +200,7 @@
wrist_goal_ = -0.62 - 0.02;
drivetrain_status_fetcher_.Fetch();
if (drivetrain_status_fetcher_.get()) {
- wrist_goal_ += drivetrain_status_fetcher_->ground_angle;
+ wrist_goal_ += drivetrain_status_fetcher_->ground_angle();
}
shooter_velocity_ = 640.0;
intake_goal_ = intake_when_shooting;
@@ -225,7 +224,7 @@
intake_goal_ = 0.0;
if (data.PosEdge(kExpand)) {
is_expanding_ = true;
- actors::SuperstructureActionParams params;
+ actors::superstructure_action::SuperstructureActionParamsT params;
params.partial_angle = 0.3;
params.delay_time = 0.7;
params.full_angle = shoulder_goal_;
@@ -248,7 +247,7 @@
bool ball_detected = false;
ball_detector_fetcher_.Fetch();
if (ball_detector_fetcher_.get()) {
- ball_detected = ball_detector_fetcher_->voltage > 2.5;
+ ball_detected = ball_detector_fetcher_->voltage() > 2.5;
}
if (data.PosEdge(kIntakeIn)) {
saw_ball_when_started_intaking_ = ball_detected;
@@ -271,16 +270,16 @@
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_action_running_ && ::std::abs(last_angle_) < 0.02 &&
::std::abs((left_goal - right_goal) -
(left_current - right_current)) /
@@ -331,54 +330,57 @@
if (!waiting_for_zero_) {
if (!is_expanding_) {
- auto new_superstructure_goal =
- superstructure_goal_sender_.MakeMessage();
- new_superstructure_goal->angle_intake = intake_goal_;
- new_superstructure_goal->angle_shoulder = shoulder_goal_;
- new_superstructure_goal->angle_wrist = wrist_goal_;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- 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;
+ ::y2016::control_loops::superstructure::Goal::Builder
+ superstructure_builder = builder.MakeBuilder<
+ ::y2016::control_loops::superstructure::Goal>();
+ superstructure_builder.add_angle_intake(intake_goal_);
+ superstructure_builder.add_angle_shoulder(shoulder_goal_);
+ superstructure_builder.add_angle_wrist(wrist_goal_);
+
+ superstructure_builder.add_max_angular_velocity_intake(7.0);
+ superstructure_builder.add_max_angular_velocity_shoulder(4.0);
+ superstructure_builder.add_max_angular_velocity_wrist(10.0);
if (use_slow_profile) {
- new_superstructure_goal->max_angular_acceleration_intake = 10.0;
+ superstructure_builder.add_max_angular_acceleration_intake(10.0);
} else {
- new_superstructure_goal->max_angular_acceleration_intake = 40.0;
+ superstructure_builder.add_max_angular_acceleration_intake(40.0);
}
- new_superstructure_goal->max_angular_acceleration_shoulder = 10.0;
+ superstructure_builder.add_max_angular_acceleration_shoulder(10.0);
if (shoulder_goal_ > 1.0) {
- new_superstructure_goal->max_angular_acceleration_wrist = 45.0;
+ superstructure_builder.add_max_angular_acceleration_wrist(45.0);
} else {
- new_superstructure_goal->max_angular_acceleration_wrist = 25.0;
+ superstructure_builder.add_max_angular_acceleration_wrist(25.0);
}
// Granny mode
/*
- new_superstructure_goal->max_angular_velocity_intake = 0.2;
- new_superstructure_goal->max_angular_velocity_shoulder = 0.2;
- new_superstructure_goal->max_angular_velocity_wrist = 0.2;
- new_superstructure_goal->max_angular_acceleration_intake = 1.0;
- new_superstructure_goal->max_angular_acceleration_shoulder = 1.0;
- new_superstructure_goal->max_angular_acceleration_wrist = 1.0;
+ superstructure_builder.add_max_angular_velocity_intake(0.2);
+ superstructure_builder.add_max_angular_velocity_shoulder(0.2);
+ superstructure_builder.add_max_angular_velocity_wrist(0.2);
+ superstructure_builder.add_max_angular_acceleration_intake(1.0);
+ superstructure_builder.add_max_angular_acceleration_shoulder(1.0);
+ superstructure_builder.add_max_angular_acceleration_wrist(1.0);
*/
if (is_intaking_) {
- new_superstructure_goal->voltage_top_rollers = 12.0;
- new_superstructure_goal->voltage_bottom_rollers = 12.0;
+ superstructure_builder.add_voltage_top_rollers(12.0);
+ superstructure_builder.add_voltage_bottom_rollers(12.0);
} else if (is_outtaking_) {
- new_superstructure_goal->voltage_top_rollers = -12.0;
- new_superstructure_goal->voltage_bottom_rollers = -7.0;
+ superstructure_builder.add_voltage_top_rollers(-12.0);
+ superstructure_builder.add_voltage_bottom_rollers(-7.0);
} else {
- new_superstructure_goal->voltage_top_rollers = 0.0;
- new_superstructure_goal->voltage_bottom_rollers = 0.0;
+ superstructure_builder.add_voltage_top_rollers(0.0);
+ superstructure_builder.add_voltage_bottom_rollers(0.0);
}
- new_superstructure_goal->traverse_unlatched = traverse_unlatched_;
- new_superstructure_goal->unfold_climber = false;
- new_superstructure_goal->voltage_climber = voltage_climber;
- new_superstructure_goal->traverse_down = traverse_down_;
- new_superstructure_goal->force_intake = true;
+ superstructure_builder.add_traverse_unlatched(traverse_unlatched_);
+ superstructure_builder.add_unfold_climber(false);
+ superstructure_builder.add_voltage_climber(voltage_climber);
+ superstructure_builder.add_traverse_down(traverse_down_);
+ superstructure_builder.add_force_intake(true);
- if (!new_superstructure_goal.Send()) {
+ if (!builder.Send(superstructure_builder.Finish())) {
AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
} else {
AOS_LOG(DEBUG, "sending goals: intake: %f, shoulder: %f, wrist: %f\n",
@@ -386,15 +388,19 @@
}
}
- auto shooter_message = shooter_goal_sender_.MakeMessage();
- shooter_message->angular_velocity = shooter_velocity_;
- shooter_message->clamp_open = is_intaking_ || is_outtaking_;
- shooter_message->push_to_shooter = fire_;
- shooter_message->force_lights_on = force_lights_on;
- shooter_message->shooting_forwards = wrist_goal_ > 0;
+ {
+ auto builder = shooter_goal_sender_.MakeBuilder();
+ y2016::control_loops::shooter::Goal::Builder shooter_builder =
+ builder.MakeBuilder<y2016::control_loops::shooter::Goal>();
+ shooter_builder.add_angular_velocity(shooter_velocity_);
+ shooter_builder.add_clamp_open(is_intaking_ || is_outtaking_);
+ shooter_builder.add_push_to_shooter(fire_);
+ shooter_builder.add_force_lights_on(force_lights_on);
+ shooter_builder.add_shooting_forwards(wrist_goal_ > 0);
- if (!shooter_message.Send()) {
- AOS_LOG(ERROR, "Sending shooter goal failed.\n");
+ if (!builder.Send(shooter_builder.Finish())) {
+ AOS_LOG(ERROR, "Sending shooter goal failed.\n");
+ }
}
}
}
@@ -402,15 +408,15 @@
private:
::aos::Fetcher<::y2016::vision::VisionStatus> vision_status_fetcher_;
::aos::Fetcher<::y2016::sensors::BallDetector> ball_detector_fetcher_;
- ::aos::Sender<::y2016::control_loops::shooter::ShooterQueue::Goal>
+ ::aos::Sender<::y2016::control_loops::shooter::Goal>
shooter_goal_sender_;
- ::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_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Goal>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Goal>
drivetrain_goal_fetcher_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
// Whatever these are set to are our default goals to send out after zeroing.
@@ -457,7 +463,10 @@
int main() {
::aos::InitNRT(true);
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::y2016::input::joysticks::Reader reader(&event_loop);
event_loop.Run();
diff --git a/y2016/queues/BUILD b/y2016/queues/BUILD
index 0c5d97b..79ec2d9 100644
--- a/y2016/queues/BUILD
+++ b/y2016/queues/BUILD
@@ -1,17 +1,11 @@
package(default_visibility = ['//visibility:public'])
-load('//aos/build:queues.bzl', 'queue_library')
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
-queue_library(
- name = 'ball_detector',
- srcs = [
- 'ball_detector.q',
- ],
-)
-
-queue_library(
- name = 'profile_params',
- srcs = [
- 'profile_params.q',
- ],
+flatbuffer_cc_library(
+ name = "ball_detector_fbs",
+ srcs = [
+ "ball_detector.fbs",
+ ],
+ gen_reflections = 1,
)
diff --git a/y2016/queues/ball_detector.q b/y2016/queues/ball_detector.fbs
similarity index 69%
rename from y2016/queues/ball_detector.q
rename to y2016/queues/ball_detector.fbs
index 9dc686f..c4ef07c 100644
--- a/y2016/queues/ball_detector.q
+++ b/y2016/queues/ball_detector.fbs
@@ -1,7 +1,7 @@
-package y2016.sensors;
+namespace y2016.sensors;
-// Published on ".y2016.sensors.ball_detector"
-message BallDetector {
+// Published on "/superstructure"
+table BallDetector {
// Voltage measured by the ball detector sensor.
// Higher voltage means ball is closer to detector, lower voltage means ball
@@ -9,5 +9,7 @@
// TODO(comran): Check to see if our sensor's output corresponds with the
// comment above.
- double voltage;
-};
+ voltage:double;
+}
+
+root_type BallDetector;
diff --git a/y2016/queues/profile_params.q b/y2016/queues/profile_params.q
deleted file mode 100644
index 56b2ab3..0000000
--- a/y2016/queues/profile_params.q
+++ /dev/null
@@ -1,6 +0,0 @@
-package y2016;
-
-struct ProfileParams {
- double velocity;
- double acceleration;
-};
diff --git a/y2016/vision/BUILD b/y2016/vision/BUILD
index 3427973..c4d472b 100644
--- a/y2016/vision/BUILD
+++ b/y2016/vision/BUILD
@@ -1,12 +1,13 @@
load("@com_google_protobuf//:protobuf.bzl", "cc_proto_library")
-load("//aos/build:queues.bzl", "queue_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
load("//tools/build_rules:gtk_dependent.bzl", "gtk_dependent_cc_binary", "gtk_dependent_cc_library")
-queue_library(
- name = "vision_queue",
+flatbuffer_cc_library(
+ name = "vision_fbs",
srcs = [
- "vision.q",
+ "vision.fbs",
],
+ gen_reflections = 1,
visibility = ["//visibility:public"],
)
@@ -130,15 +131,15 @@
deps = [
":stereo_geometry",
":vision_data",
- ":vision_queue",
+ ":vision_fbs",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
"//aos/logging",
- "//aos/logging:queue_logging",
"//aos/mutex",
"//aos/time",
"//aos/vision/events:udp",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//y2016:constants",
],
)
diff --git a/y2016/vision/target_receiver.cc b/y2016/vision/target_receiver.cc
index a66be59..b946f8b 100644
--- a/y2016/vision/target_receiver.cc
+++ b/y2016/vision/target_receiver.cc
@@ -10,19 +10,18 @@
#include <thread>
#include <vector>
-#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/mutex/mutex.h"
#include "aos/time/time.h"
#include "aos/vision/events/udp.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "y2016/constants.h"
#include "y2016/vision/stereo_geometry.h"
-#include "y2016/vision/vision.q.h"
#include "y2016/vision/vision_data.pb.h"
+#include "y2016/vision/vision_generated.h"
namespace y2016 {
namespace vision {
@@ -197,17 +196,20 @@
DrivetrainOffsetCalculator(::aos::EventLoop *event_loop)
: drivetrain_status_fetcher_(
event_loop
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
- ".frc971.control_loops.drivetrain_queue.status")) {}
+ ->MakeFetcher<::frc971::control_loops::drivetrain::Status>(
+ "/drivetrain")) {}
// Takes a vision status message with everything except
// drivetrain_{left,right}_position set and sets those.
// Returns false if it doesn't have enough data to fill them out.
- bool CompleteVisionStatus(::y2016::vision::VisionStatus *status) {
+ bool CompleteVisionStatus(::y2016::vision::VisionStatusT *status) {
while (drivetrain_status_fetcher_.FetchNext()) {
- data_[data_index_].time = drivetrain_status_fetcher_->sent_time;
- data_[data_index_].left = drivetrain_status_fetcher_->estimated_left_position;
- data_[data_index_].right = drivetrain_status_fetcher_->estimated_right_position;
+ data_[data_index_].time =
+ drivetrain_status_fetcher_.context().monotonic_sent_time;
+ data_[data_index_].left =
+ drivetrain_status_fetcher_->estimated_left_position();
+ data_[data_index_].right =
+ drivetrain_status_fetcher_->estimated_right_position();
++data_index_;
if (data_index_ == data_.size()) data_index_ = 0;
if (valid_data_ < data_.size()) ++valid_data_;
@@ -286,7 +288,7 @@
}
}
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
::std::array<DrivetrainData, 200> data_;
@@ -297,11 +299,13 @@
};
void Main() {
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::aos::Sender<::y2016::vision::VisionStatus> vision_status_sender =
- event_loop.MakeSender<::y2016::vision::VisionStatus>(
- ".y2016.vision.vision_status");
+ event_loop.MakeSender<::y2016::vision::VisionStatus>("/superstructure");
StereoGeometry stereo(constants::GetValues().vision_name);
AOS_LOG(INFO, "calibration: %s\n",
@@ -339,9 +343,10 @@
const bool left_image_valid = left.is_valid();
const bool right_image_valid = right.is_valid();
- auto new_vision_status = vision_status_sender.MakeMessage();
- new_vision_status->left_image_valid = left_image_valid;
- new_vision_status->right_image_valid = right_image_valid;
+ auto builder = vision_status_sender.MakeBuilder();
+ VisionStatusT new_vision_status;
+ new_vision_status.left_image_valid = left_image_valid;
+ new_vision_status.right_image_valid = right_image_valid;
if (left_image_valid && right_image_valid) {
::aos::vision::Vector<2> center_left(0.0, 0.0);
::aos::vision::Vector<2> center_right(0.0, 0.0);
@@ -367,7 +372,7 @@
if (left.capture_time() < right.capture_time()) {
filtered_center_left = center_left;
filtered_angle_left = angle_left;
- new_vision_status->target_time =
+ new_vision_status.target_time =
chrono::duration_cast<chrono::nanoseconds>(
left.capture_time().time_since_epoch())
.count();
@@ -377,7 +382,7 @@
} else {
filtered_center_right = center_right;
filtered_angle_right = angle_right;
- new_vision_status->target_time =
+ new_vision_status.target_time =
chrono::duration_cast<chrono::nanoseconds>(
right.capture_time().time_since_epoch())
.count();
@@ -389,28 +394,27 @@
double distance, horizontal_angle, vertical_angle;
stereo.Process(filtered_center_left, filtered_center_right, &distance,
&horizontal_angle, &vertical_angle);
- new_vision_status->left_image_timestamp =
+ new_vision_status.left_image_timestamp =
left.target().image_timestamp();
- new_vision_status->right_image_timestamp =
+ new_vision_status.right_image_timestamp =
right.target().image_timestamp();
- new_vision_status->left_send_timestamp = left.target().send_timestamp();
- new_vision_status->right_send_timestamp =
+ new_vision_status.left_send_timestamp = left.target().send_timestamp();
+ new_vision_status.right_send_timestamp =
right.target().send_timestamp();
- new_vision_status->horizontal_angle = horizontal_angle;
- new_vision_status->vertical_angle = vertical_angle;
- new_vision_status->distance = distance;
- new_vision_status->angle =
+ new_vision_status.horizontal_angle = horizontal_angle;
+ new_vision_status.vertical_angle = vertical_angle;
+ new_vision_status.distance = distance;
+ new_vision_status.angle =
(filtered_angle_left + filtered_angle_right) / 2.0;
}
- if (drivetrain_offset.CompleteVisionStatus(new_vision_status.get())) {
- AOS_LOG_STRUCT(DEBUG, "vision", *new_vision_status);
- if (!new_vision_status.Send()) {
+ if (drivetrain_offset.CompleteVisionStatus(&new_vision_status)) {
+ if (!builder.Send(
+ VisionStatus::Pack(*builder.fbb(), &new_vision_status))) {
AOS_LOG(ERROR, "Failed to send vision information\n");
}
} else {
- AOS_LOG_STRUCT(WARNING, "vision without drivetrain",
- *new_vision_status);
+ AOS_LOG(WARNING, "vision without drivetrain");
}
}
diff --git a/y2016/vision/vision.q b/y2016/vision/vision.fbs
similarity index 65%
rename from y2016/vision/vision.q
rename to y2016/vision/vision.fbs
index 06deb1f..4394b10 100644
--- a/y2016/vision/vision.q
+++ b/y2016/vision/vision.fbs
@@ -1,36 +1,38 @@
-package y2016.vision;
+namespace y2016.vision;
// Published on ".y2016.vision.vision_status"
-message VisionStatus {
- bool left_image_valid;
- bool right_image_valid;
+table VisionStatus {
+ left_image_valid:bool;
+ right_image_valid:bool;
// Times when the images were taken as nanoseconds on CLOCK_MONOTONIC on the
// TK1.
- int64_t left_image_timestamp;
- int64_t right_image_timestamp;
+ left_image_timestamp:long;
+ right_image_timestamp:long;
// Times when the images were sent from the TK1 as nanoseconds on the TK1's
// CLOCK_MONOTONIC.
- int64_t left_send_timestamp;
- int64_t right_send_timestamp;
+ left_send_timestamp:long;
+ right_send_timestamp:long;
// Horizontal angle of the goal in radians.
// TODO(Brian): Figure out which way is positive.
- double horizontal_angle;
+ horizontal_angle:double;
// Vertical angle of the goal in radians.
// TODO(Brian): Figure out which way is positive.
- double vertical_angle;
+ vertical_angle:double;
// Distance to the target in meters.
- double distance;
+ distance:double;
// The angle in radians of the bottom of the target.
- double angle;
+ angle:double;
// Capture time of the angle using the clock behind monotonic_clock::now().
- int64_t target_time;
+ target_time:long;
// The estimated positions of both sides of the drivetrain when the frame
// was captured.
// These are the estimated_left_position and estimated_right_position members
// of the drivetrain queue.
- double drivetrain_left_position;
- double drivetrain_right_position;
-};
+ drivetrain_left_position:double;
+ drivetrain_right_position:double;
+}
+
+root_type VisionStatus;
diff --git a/y2016/wpilib_interface.cc b/y2016/wpilib_interface.cc
index 9b31a0f..c36bab1 100644
--- a/y2016/wpilib_interface.cc
+++ b/y2016/wpilib_interface.cc
@@ -20,18 +20,18 @@
#undef ERROR
#include "aos/commonmath.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.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/robot_state/robot_state_generated.h"
#include "aos/stl_mutex/stl_mutex.h"
#include "aos/time/time.h"
#include "aos/util/log_interval.h"
#include "aos/util/wrapping_counter.h"
-#include "frc971/autonomous/auto.q.h"
-#include "frc971/control_loops/control_loops.q.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/autonomous/auto_generated.h"
+//#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_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"
@@ -42,21 +42,22 @@
#include "frc971/wpilib/gyro_sender.h"
#include "frc971/wpilib/interrupt_edge_counting.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 "y2016/constants.h"
#include "y2016/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2016/control_loops/shooter/shooter.q.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_output_generated.h"
+#include "y2016/control_loops/shooter/shooter_position_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2016/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2016/queues/ball_detector_generated.h"
using aos::make_unique;
using ::frc971::wpilib::LoopOutputHandler;
-using ::y2016::control_loops::shooter::ShooterQueue;
-using ::y2016::control_loops::SuperstructureQueue;
+namespace shooter = ::y2016::control_loops::shooter;
+namespace superstructure = ::y2016::control_loops::superstructure;
namespace y2016 {
namespace wpilib {
@@ -149,19 +150,19 @@
: ::frc971::wpilib::SensorReader(event_loop),
ball_detector_sender_(
event_loop->MakeSender<::y2016::sensors::BallDetector>(
- ".y2016.sensors.ball_detector")),
+ "/superstructure")),
auto_mode_sender_(
event_loop->MakeSender<::frc971::autonomous::AutonomousMode>(
- ".frc971.autonomous.auto_mode")),
- shooter_position_sender_(event_loop->MakeSender<ShooterQueue::Position>(
- ".y2016.control_loops.shooter.shooter_queue.position")),
+ "/aos")),
+ shooter_position_sender_(
+ event_loop->MakeSender<shooter::Position>("/shooter")),
superstructure_position_sender_(
- event_loop->MakeSender<SuperstructureQueue::Position>(
- ".y2016.control_loops.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 it to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateFastEncoderFilterHz(kMaxDrivetrainShooterEncoderPulsesPerSecond);
@@ -258,78 +259,101 @@
void RunIteration() {
{
- auto drivetrain_message = drivetrain_position_sender_.MakeMessage();
- drivetrain_message->right_encoder =
- drivetrain_translate(-drivetrain_right_encoder_->GetRaw());
- drivetrain_message->left_encoder =
- -drivetrain_translate(drivetrain_left_encoder_->GetRaw());
- drivetrain_message->left_speed =
- drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod());
- drivetrain_message->right_speed =
- drivetrain_velocity_translate(drivetrain_right_encoder_->GetPeriod());
+ auto builder = drivetrain_position_sender_.MakeBuilder();
+ frc971::control_loops::drivetrain::Position::Builder position_builder =
+ builder.MakeBuilder<frc971::control_loops::drivetrain::Position>();
- drivetrain_message->left_shifter_position =
- hall_translate(drivetrain_left_hall_->GetVoltage());
- drivetrain_message->right_shifter_position =
- hall_translate(drivetrain_right_hall_->GetVoltage());
+ position_builder.add_right_encoder(
+ drivetrain_translate(-drivetrain_right_encoder_->GetRaw()));
+ position_builder.add_left_encoder(
+ -drivetrain_translate(drivetrain_left_encoder_->GetRaw()));
+ position_builder.add_left_speed(
+ drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod()));
+ position_builder.add_right_speed(drivetrain_velocity_translate(
+ drivetrain_right_encoder_->GetPeriod()));
- drivetrain_message.Send();
+ position_builder.add_left_shifter_position(
+ hall_translate(drivetrain_left_hall_->GetVoltage()));
+ position_builder.add_right_shifter_position(
+ hall_translate(drivetrain_right_hall_->GetVoltage()));
+
+ builder.Send(position_builder.Finish());
}
}
void RunDmaIteration() {
const auto &values = constants::GetValues();
{
- auto shooter_message = shooter_position_sender_.MakeMessage();
- shooter_message->theta_left =
- shooter_translate(-shooter_left_encoder_->GetRaw());
- shooter_message->theta_right =
- shooter_translate(shooter_right_encoder_->GetRaw());
- shooter_message.Send();
+ auto builder = shooter_position_sender_.MakeBuilder();
+ shooter::Position::Builder shooter_builder =
+ builder.MakeBuilder<shooter::Position>();
+ shooter_builder.add_theta_left(
+ shooter_translate(-shooter_left_encoder_->GetRaw()));
+ shooter_builder.add_theta_right(
+ shooter_translate(shooter_right_encoder_->GetRaw()));
+ builder.Send(shooter_builder.Finish());
}
{
- auto superstructure_message =
- superstructure_position_sender_.MakeMessage();
- CopyPosition(intake_encoder_, &superstructure_message->intake,
- intake_translate, intake_pot_translate, false,
- values.intake.pot_offset);
- CopyPosition(shoulder_encoder_, &superstructure_message->shoulder,
- shoulder_translate, shoulder_pot_translate, false,
- values.shoulder.pot_offset);
- CopyPosition(wrist_encoder_, &superstructure_message->wrist,
- wrist_translate, wrist_pot_translate, true,
- values.wrist.pot_offset);
+ auto builder = superstructure_position_sender_.MakeBuilder();
- superstructure_message.Send();
+ frc971::PotAndIndexPositionT intake;
+ CopyPosition(intake_encoder_, &intake, intake_translate,
+ intake_pot_translate, false, values.intake.pot_offset);
+ flatbuffers::Offset<frc971::PotAndIndexPosition> intake_offset =
+ frc971::PotAndIndexPosition::Pack(*builder.fbb(), &intake);
+
+ frc971::PotAndIndexPositionT shoulder;
+ CopyPosition(shoulder_encoder_, &shoulder, shoulder_translate,
+ shoulder_pot_translate, false, values.shoulder.pot_offset);
+ flatbuffers::Offset<frc971::PotAndIndexPosition> shoulder_offset =
+ frc971::PotAndIndexPosition::Pack(*builder.fbb(), &shoulder);
+
+ frc971::PotAndIndexPositionT wrist;
+ CopyPosition(wrist_encoder_, &wrist, wrist_translate, wrist_pot_translate,
+ true, values.wrist.pot_offset);
+ flatbuffers::Offset<frc971::PotAndIndexPosition> wrist_offset =
+ frc971::PotAndIndexPosition::Pack(*builder.fbb(), &wrist);
+
+ superstructure::Position::Builder position_builder =
+ builder.MakeBuilder<superstructure::Position>();
+
+ position_builder.add_intake(intake_offset);
+ position_builder.add_shoulder(shoulder_offset);
+ position_builder.add_wrist(wrist_offset);
+
+ builder.Send(position_builder.Finish());
}
{
- auto ball_detector_message = ball_detector_sender_.MakeMessage();
- ball_detector_message->voltage = ball_detector_->GetVoltage();
- AOS_LOG_STRUCT(DEBUG, "ball detector", *ball_detector_message);
- ball_detector_message.Send();
+ auto builder = ball_detector_sender_.MakeBuilder();
+ ::y2016::sensors::BallDetector::Builder ball_detector_builder =
+ builder.MakeBuilder<y2016::sensors::BallDetector>();
+ ball_detector_builder.add_voltage(ball_detector_->GetVoltage());
+ builder.Send(ball_detector_builder.Finish());
}
{
- auto auto_mode_message = auto_mode_sender_.MakeMessage();
- auto_mode_message->mode = 0;
+ auto builder = auto_mode_sender_.MakeBuilder();
+ ::frc971::autonomous::AutonomousMode::Builder auto_builder =
+ builder.MakeBuilder<frc971::autonomous::AutonomousMode>();
+ int mode = 0;
for (size_t i = 0; i < autonomous_modes_.size(); ++i) {
if (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_builder.add_mode(mode);
+ builder.Send(auto_builder.Finish());
}
}
private:
::aos::Sender<::y2016::sensors::BallDetector> ball_detector_sender_;
::aos::Sender<::frc971::autonomous::AutonomousMode> auto_mode_sender_;
- ::aos::Sender<ShooterQueue::Position> shooter_position_sender_;
- ::aos::Sender<SuperstructureQueue::Position> superstructure_position_sender_;
- ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Position>
+ ::aos::Sender<shooter::Position> shooter_position_sender_;
+ ::aos::Sender<superstructure::Position> superstructure_position_sender_;
+ ::aos::Sender<::frc971::control_loops::drivetrain::Position>
drivetrain_position_sender_;
::std::unique_ptr<::frc::AnalogInput> drivetrain_left_hall_,
@@ -349,17 +373,19 @@
class SolenoidWriter {
public:
SolenoidWriter(::aos::EventLoop *event_loop,
- const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm)
+ ::frc971::wpilib::BufferedPcm *pcm)
: pcm_(pcm),
+ pneumatics_to_log_sender_(
+ event_loop->MakeSender<::frc971::wpilib::PneumaticsToLog>("/aos")),
drivetrain_(
event_loop
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Output>(
- ".frc971.control_loops.drivetrain_queue.output")),
- shooter_(event_loop->MakeFetcher<ShooterQueue::Output>(
- ".y2016.control_loops.shooter.shooter_queue.output")),
- superstructure_(event_loop->MakeFetcher<
- ::y2016::control_loops::SuperstructureQueue::Output>(
- ".y2016.control_loops.superstructure_queue.output")) {
+ ->MakeFetcher<::frc971::control_loops::drivetrain::Output>(
+ "/drivetrain")),
+ shooter_(event_loop->MakeFetcher<shooter::Output>("/shooter")),
+ superstructure_(
+ event_loop
+ ->MakeFetcher<::y2016::control_loops::superstructure::Output>(
+ "/superstructure")) {
event_loop->set_name("Solenoids");
event_loop->SetRuntimeRealtimePriority(27);
@@ -420,27 +446,25 @@
{
drivetrain_.Fetch();
if (drivetrain_.get()) {
- AOS_LOG_STRUCT(DEBUG, "solenoids", *drivetrain_);
drivetrain_shifter_->Set(
- !(drivetrain_->left_high || drivetrain_->right_high));
+ !(drivetrain_->left_high() || drivetrain_->right_high()));
}
}
{
shooter_.Fetch();
if (shooter_.get()) {
- AOS_LOG_STRUCT(DEBUG, "solenoids", *shooter_);
- shooter_clamp_->Set(shooter_->clamp_open);
- shooter_pusher_->Set(shooter_->push_to_shooter);
- lights_->Set(shooter_->lights_on);
- if (shooter_->forwards_flashlight) {
- if (shooter_->backwards_flashlight) {
+ shooter_clamp_->Set(shooter_->clamp_open());
+ shooter_pusher_->Set(shooter_->push_to_shooter());
+ lights_->Set(shooter_->lights_on());
+ if (shooter_->forwards_flashlight()) {
+ if (shooter_->backwards_flashlight()) {
flashlight_->Set(::frc::Relay::kOn);
} else {
flashlight_->Set(::frc::Relay::kReverse);
}
} else {
- if (shooter_->backwards_flashlight) {
+ if (shooter_->backwards_flashlight()) {
flashlight_->Set(::frc::Relay::kForward);
} else {
flashlight_->Set(::frc::Relay::kOff);
@@ -452,26 +476,29 @@
{
superstructure_.Fetch();
if (superstructure_.get()) {
- AOS_LOG_STRUCT(DEBUG, "solenoids", *superstructure_);
+ climber_trigger_->Set(superstructure_->unfold_climber());
- climber_trigger_->Set(superstructure_->unfold_climber);
-
- traverse_->Set(superstructure_->traverse_down);
- traverse_latch_->Set(superstructure_->traverse_unlatched);
+ traverse_->Set(superstructure_->traverse_down());
+ traverse_latch_->Set(superstructure_->traverse_unlatched());
}
}
{
- ::frc971::wpilib::PneumaticsToLog to_log;
- { to_log.compressor_on = compressor_->Enabled(); }
+ auto builder = pneumatics_to_log_sender_.MakeBuilder();
+
+ ::frc971::wpilib::PneumaticsToLog::Builder to_log_builder =
+ builder.MakeBuilder<frc971::wpilib::PneumaticsToLog>();
+
+ to_log_builder.add_compressor_on(compressor_->Enabled());
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());
}
}
- const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm_;
+ ::frc971::wpilib::BufferedPcm *pcm_;
+ aos::Sender<::frc971::wpilib::PneumaticsToLog> pneumatics_to_log_sender_;
::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> drivetrain_shifter_,
shooter_clamp_, shooter_pusher_, lights_, traverse_, traverse_latch_,
@@ -479,17 +506,16 @@
::std::unique_ptr<::frc::Relay> flashlight_;
::std::unique_ptr<::frc::Compressor> compressor_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Output> drivetrain_;
- ::aos::Fetcher<ShooterQueue::Output> shooter_;
- ::aos::Fetcher<::y2016::control_loops::SuperstructureQueue::Output>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Output> drivetrain_;
+ ::aos::Fetcher<shooter::Output> shooter_;
+ ::aos::Fetcher<::y2016::control_loops::superstructure::Output>
superstructure_;
};
-class ShooterWriter : public LoopOutputHandler<ShooterQueue::Output> {
+class ShooterWriter : public LoopOutputHandler<shooter::Output> {
public:
ShooterWriter(::aos::EventLoop *event_loop)
- : LoopOutputHandler<ShooterQueue::Output>(
- event_loop, ".y2016.control_loops.shooter.shooter_queue.output") {}
+ : LoopOutputHandler<shooter::Output>(event_loop, "/shooter") {}
void set_shooter_left_talon(::std::unique_ptr<::frc::Talon> t) {
shooter_left_talon_ = ::std::move(t);
@@ -500,11 +526,9 @@
}
private:
- void Write(const ShooterQueue::Output &output) override {
- AOS_LOG_STRUCT(DEBUG, "will output", output);
-
- shooter_left_talon_->SetSpeed(output.voltage_left / 12.0);
- shooter_right_talon_->SetSpeed(-output.voltage_right / 12.0);
+ void Write(const shooter::Output &output) override {
+ shooter_left_talon_->SetSpeed(output.voltage_left() / 12.0);
+ shooter_right_talon_->SetSpeed(-output.voltage_right() / 12.0);
}
void Stop() override {
@@ -516,13 +540,11 @@
::std::unique_ptr<::frc::Talon> shooter_left_talon_, shooter_right_talon_;
};
-class SuperstructureWriter
- : public LoopOutputHandler<
- ::y2016::control_loops::SuperstructureQueue::Output> {
+class SuperstructureWriter : public LoopOutputHandler<superstructure::Output> {
public:
SuperstructureWriter(::aos::EventLoop *event_loop)
- : LoopOutputHandler<::y2016::control_loops::SuperstructureQueue::Output>(
- event_loop, ".y2016.control_loops.superstructure_queue.output") {}
+ : LoopOutputHandler<superstructure::Output>(event_loop,
+ "/superstructure") {}
void set_intake_talon(::std::unique_ptr<::frc::Talon> t) {
intake_talon_ = ::std::move(t);
@@ -549,21 +571,19 @@
}
private:
- virtual void Write(const ::y2016::control_loops::SuperstructureQueue::Output
- &output) override {
- AOS_LOG_STRUCT(DEBUG, "will output", output);
- intake_talon_->SetSpeed(::aos::Clip(output.voltage_intake,
+ virtual void Write(const superstructure::Output &output) override {
+ intake_talon_->SetSpeed(::aos::Clip(output.voltage_intake(),
-kMaxBringupPower, kMaxBringupPower) /
12.0);
- shoulder_talon_->SetSpeed(::aos::Clip(-output.voltage_shoulder,
+ shoulder_talon_->SetSpeed(::aos::Clip(-output.voltage_shoulder(),
-kMaxBringupPower, kMaxBringupPower) /
12.0);
- wrist_talon_->SetSpeed(
- ::aos::Clip(output.voltage_wrist, -kMaxBringupPower, kMaxBringupPower) /
- 12.0);
- top_rollers_talon_->SetSpeed(-output.voltage_top_rollers / 12.0);
- bottom_rollers_talon_->SetSpeed(-output.voltage_bottom_rollers / 12.0);
- climber_talon_->SetSpeed(-output.voltage_climber / 12.0);
+ wrist_talon_->SetSpeed(::aos::Clip(output.voltage_wrist(),
+ -kMaxBringupPower, kMaxBringupPower) /
+ 12.0);
+ top_rollers_talon_->SetSpeed(-output.voltage_top_rollers() / 12.0);
+ bottom_rollers_talon_->SetSpeed(-output.voltage_bottom_rollers() / 12.0);
+ climber_talon_->SetSpeed(-output.voltage_climber() / 12.0);
}
virtual void Stop() override {
@@ -585,19 +605,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(5));
@@ -631,19 +654,19 @@
// TODO(Brian): This interacts poorly with the SpiRxClearer in ADIS16448.
// Thread 4.
- ::aos::ShmEventLoop gyro_event_loop;
+ ::aos::ShmEventLoop gyro_event_loop(&config.message());
::frc971::wpilib::GyroSender gyro_sender(&gyro_event_loop);
AddLoop(&gyro_event_loop);
// Thread 5.
- ::aos::ShmEventLoop imu_event_loop;
+ ::aos::ShmEventLoop imu_event_loop(&config.message());
auto imu_trigger = make_unique<::frc::DigitalInput>(3);
::frc971::wpilib::ADIS16448 imu(&imu_event_loop, ::frc::SPI::Port::kMXP,
imu_trigger.get());
AddLoop(&imu_event_loop);
// 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::Talon>(new ::frc::Talon(5)), false);
@@ -673,10 +696,10 @@
AddLoop(&output_event_loop);
// Thread 6.
- ::aos::ShmEventLoop solenoid_writer_event_loop;
+ ::aos::ShmEventLoop solenoid_writer_event_loop(&config.message());
::std::unique_ptr<::frc971::wpilib::BufferedPcm> pcm(
new ::frc971::wpilib::BufferedPcm());
- SolenoidWriter solenoid_writer(&solenoid_writer_event_loop, pcm);
+ SolenoidWriter solenoid_writer(&solenoid_writer_event_loop, pcm.get());
solenoid_writer.set_drivetrain_shifter(pcm->MakeSolenoid(0));
solenoid_writer.set_traverse_latch(pcm->MakeSolenoid(2));
solenoid_writer.set_traverse(pcm->MakeSolenoid(3));
diff --git a/y2016/y2016.json b/y2016/y2016.json
new file mode 100644
index 0000000..eb8ecd5
--- /dev/null
+++ b/y2016/y2016.json
@@ -0,0 +1,54 @@
+{
+ "channels":
+ [
+ {
+ "name": "/shooter",
+ "type": "y2016.control_loops.shooter.Goal",
+ "frequency": 200
+ },
+ {
+ "name": "/shooter",
+ "type": "y2016.control_loops.shooter.Status",
+ "frequency": 200
+ },
+ {
+ "name": "/shooter",
+ "type": "y2016.control_loops.shooter.Output",
+ "frequency": 200
+ },
+ {
+ "name": "/shooter",
+ "type": "y2016.control_loops.shooter.Position",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2016.control_loops.superstructure.Goal",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2016.control_loops.superstructure.Status",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2016.control_loops.superstructure.Output",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2016.control_loops.superstructure.Position",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2016.sensors.BallDetector",
+ "frequency": 200
+ }
+ ],
+ "imports": [
+ "../aos/robot_state/robot_state_config.json",
+ "../frc971/control_loops/drivetrain/drivetrain_config.json"
+ ]
+}