Convert aos over to flatbuffers
Everything builds, and all the tests pass. I suspect that some entries
are missing from the config files, but those will be found pretty
quickly on startup.
There is no logging or live introspection of queue messages.
Change-Id: I496ee01ed68f202c7851bed7e8786cee30df29f5
diff --git a/y2019/BUILD b/y2019/BUILD
index 04855d5..5c9a3a6 100644
--- a/y2019/BUILD
+++ b/y2019/BUILD
@@ -1,6 +1,7 @@
load("//frc971:downloader.bzl", "robot_downloader")
-load("//aos/build:queues.bzl", "queue_library")
+load("//aos:config.bzl", "aos_config")
load("@com_google_protobuf//:protobuf.bzl", "cc_proto_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
robot_downloader(
dirs = [
@@ -56,25 +57,24 @@
"//aos:make_unique",
"//aos:math",
"//aos/controls:control_loop",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_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_position_fbs",
"//frc971/wpilib:ADIS16448",
"//frc971/wpilib:buffered_pcm",
"//frc971/wpilib:drivetrain_writer",
"//frc971/wpilib:encoder_and_potentiometer",
"//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",
@@ -82,8 +82,9 @@
"//frc971/wpilib:wpilib_robot_base",
"//third_party:phoenix",
"//third_party:wpilib",
- "//y2019/control_loops/drivetrain:camera_queue",
- "//y2019/control_loops/superstructure:superstructure_queue",
+ "//y2019/control_loops/drivetrain:camera_fbs",
+ "//y2019/control_loops/superstructure:superstructure_output_fbs",
+ "//y2019/control_loops/superstructure:superstructure_position_fbs",
"//y2019/jevois:spi",
],
)
@@ -132,23 +133,45 @@
"//aos/time",
"//aos/util:log_interval",
"//aos/vision/events:udp",
- "//frc971/autonomous:auto_queue",
+ "//frc971/autonomous:auto_fbs",
"//frc971/autonomous:base_autonomous_actor",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
- "//frc971/control_loops/drivetrain:localizer_queue",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//frc971/control_loops/drivetrain:localizer_fbs",
"//y2019/control_loops/drivetrain:drivetrain_base",
- "//y2019/control_loops/drivetrain:target_selector_queue",
- "//y2019/control_loops/superstructure:superstructure_queue",
+ "//y2019/control_loops/drivetrain:target_selector_fbs",
+ "//y2019/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2019/control_loops/superstructure:superstructure_position_fbs",
+ "//y2019/control_loops/superstructure:superstructure_status_fbs",
"@com_google_protobuf//:protobuf",
],
)
-queue_library(
+flatbuffer_cc_library(
name = "status_light",
srcs = [
- "status_light.q",
+ "status_light.fbs",
+ ],
+ gen_reflections = 1,
+ visibility = ["//visibility:public"],
+)
+
+aos_config(
+ name = "config",
+ src = "y2019.json",
+ flatbuffers = [
+ ":status_light",
+ "//y2019/control_loops/drivetrain:camera_fbs",
+ "//y2019/control_loops/drivetrain:target_selector_fbs",
+ "//y2019/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2019/control_loops/superstructure:superstructure_output_fbs",
+ "//y2019/control_loops/superstructure:superstructure_position_fbs",
+ "//y2019/control_loops/superstructure:superstructure_status_fbs",
],
visibility = ["//visibility:public"],
+ deps = [
+ "//aos/robot_state:config",
+ "//frc971/control_loops/drivetrain:config",
+ ],
)
cc_proto_library(
diff --git a/y2019/actors/BUILD b/y2019/actors/BUILD
index a622dbf..3aa2a22 100644
--- a/y2019/actors/BUILD
+++ b/y2019/actors/BUILD
@@ -25,16 +25,17 @@
"autonomous_actor.h",
],
deps = [
- "//aos/events:event-loop",
+ "//aos/events:event_loop",
"//aos/logging",
"//aos/util:phased_loop",
"//frc971/autonomous:base_autonomous_actor",
- "//frc971/control_loops:queues",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
"//frc971/control_loops/drivetrain:drivetrain_config",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
- "//frc971/control_loops/drivetrain:localizer_queue",
+ "//frc971/control_loops/drivetrain:localizer_fbs",
"//y2019/control_loops/drivetrain:drivetrain_base",
- "//y2019/control_loops/superstructure:superstructure_queue",
+ "//y2019/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2019/control_loops/superstructure:superstructure_status_fbs",
],
)
@@ -46,7 +47,7 @@
deps = [
":autonomous_action_lib",
"//aos:init",
- "//aos/events:shm-event-loop",
- "//frc971/autonomous:auto_queue",
+ "//aos/events:shm_event_loop",
+ "//frc971/autonomous:auto_fbs",
],
)
diff --git a/y2019/actors/auto_splines.cc b/y2019/actors/auto_splines.cc
index 4e8e60d..6166ca8 100644
--- a/y2019/actors/auto_splines.cc
+++ b/y2019/actors/auto_splines.cc
@@ -1,397 +1,792 @@
#include "y2019/actors/auto_splines.h"
-#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
namespace y2019 {
namespace actors {
-void MaybeFlipSpline(::frc971::MultiSpline *spline, bool is_left) {
+void MaybeFlipSpline(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset,
+ bool is_left) {
+
+ flatbuffers::Vector<float> *spline_y =
+ GetMutableTemporaryPointer(*builder->fbb(), spline_y_offset);
+
if (!is_left) {
- for (size_t i = 0; i < spline->spline_y.size(); i++) {
- spline->spline_y[i] *= -1.0;
+ for (size_t i = 0; i < spline_y->size(); i++) {
+ spline_y->Mutate(i, -spline_y->Get(i));
}
}
}
// Path off of level 2 to the far side of the rocket with a panel
-::frc971::MultiSpline AutonomousSplines::HABToFarRocket(bool is_left) {
- ::frc971::MultiSpline spline;
- ::frc971::Constraint longitudinal_constraint;
- ::frc971::Constraint lateral_constraint;
- ::frc971::Constraint voltage_constraint;
- ::frc971::Constraint velocity_constraint;
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::HABToFarRocket(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ bool is_left) {
+ flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> velocity_constraint_offset;
- longitudinal_constraint.constraint_type = 1;
- longitudinal_constraint.value = 2.0;
+ {
+ frc971::Constraint::Builder longitudinal_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ longitudinal_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_LONGITUDINAL_ACCELERATION);
+ longitudinal_constraint_builder.add_value(2.0);
+ longitudinal_constraint_offset = longitudinal_constraint_builder.Finish();
+ }
- lateral_constraint.constraint_type = 2;
- lateral_constraint.value = 2.0;
+ {
+ frc971::Constraint::Builder lateral_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ lateral_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_LATERAL_ACCELERATION);
+ lateral_constraint_builder.add_value(2.0);
+ lateral_constraint_offset = lateral_constraint_builder.Finish();
+ }
- voltage_constraint.constraint_type = 3;
- voltage_constraint.value = 11.0;
+ {
+ frc971::Constraint::Builder voltage_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ voltage_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VOLTAGE);
+ voltage_constraint_builder.add_value(11.0);
+ voltage_constraint_offset = voltage_constraint_builder.Finish();
+ }
- velocity_constraint.constraint_type = 4;
- velocity_constraint.value = 4.0;
- velocity_constraint.start_distance = 0.0;
- velocity_constraint.end_distance = 10.0;
+ {
+ frc971::Constraint::Builder velocity_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ velocity_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VELOCITY);
+ velocity_constraint_builder.add_value(4.0);
+ velocity_constraint_builder.add_start_distance(0.0);
+ velocity_constraint_builder.add_end_distance(10.0);
+ velocity_constraint_offset = velocity_constraint_builder.Finish();
+ }
- spline.spline_count = 2;
- spline.spline_x = {{1.14763818102, 1.66, 3.10, 4.05, 4.45, 5.11, 5.77, 6.71,
- 7.27, 7.19, 6.57}};
- spline.spline_y = {{1.30261224364, 1.30217320136, 1.39, 1.47, 1.56346705393,
- 1.69, 1.81, 1.97, 2.18, 2.84, 3.33}};
+ flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
+ constraints_offset =
+ builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
+ {longitudinal_constraint_offset, lateral_constraint_offset,
+ voltage_constraint_offset, velocity_constraint_offset});
- spline.constraints = {{longitudinal_constraint, lateral_constraint,
- voltage_constraint, velocity_constraint}};
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder->fbb()->CreateVector<float>({1.14763818102, 1.66, 3.10, 4.05,
+ 4.45, 5.11, 5.77, 6.71, 7.27, 7.19,
+ 6.57});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder->fbb()->CreateVector<float>({1.30261224364, 1.30217320136, 1.39,
+ 1.47, 1.56346705393, 1.69, 1.81,
+ 1.97, 2.18, 2.84, 3.33});
+ MaybeFlipSpline(builder, spline_y_offset, is_left);
- MaybeFlipSpline(&spline, is_left);
- return spline;
+ frc971::MultiSpline::Builder multispline_builder =
+ builder->MakeBuilder<frc971::MultiSpline>();
+
+ multispline_builder.add_spline_count(2);
+ multispline_builder.add_constraints(constraints_offset);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ return multispline_builder.Finish();
}
// Path from the far side of the rocket to the loading station to pickup
-::frc971::MultiSpline AutonomousSplines::FarRocketToHP(bool is_left) {
- ::frc971::MultiSpline spline;
- ::frc971::Constraint longitudinal_constraint;
- ::frc971::Constraint lateral_constraint;
- ::frc971::Constraint voltage_constraint;
- ::frc971::Constraint velocity_constraint;
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::FarRocketToHP(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ bool is_left) {
+ flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> velocity_constraint_offset;
- longitudinal_constraint.constraint_type = 1;
- longitudinal_constraint.value = 3.0;
+ {
+ frc971::Constraint::Builder longitudinal_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ longitudinal_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_LONGITUDINAL_ACCELERATION);
+ longitudinal_constraint_builder.add_value(3.0);
+ longitudinal_constraint_offset = longitudinal_constraint_builder.Finish();
+ }
- lateral_constraint.constraint_type = 2;
- lateral_constraint.value = 2.0;
+ {
+ frc971::Constraint::Builder lateral_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ lateral_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_LATERAL_ACCELERATION);
+ lateral_constraint_builder.add_value(2.0);
+ lateral_constraint_offset = lateral_constraint_builder.Finish();
+ }
- voltage_constraint.constraint_type = 3;
- voltage_constraint.value = 11.0;
+ {
+ frc971::Constraint::Builder voltage_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ voltage_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VOLTAGE);
+ voltage_constraint_builder.add_value(11.0);
+ voltage_constraint_offset = voltage_constraint_builder.Finish();
+ }
- velocity_constraint.constraint_type = 4;
- velocity_constraint.value = 4.5;
- velocity_constraint.start_distance = 0.0;
- velocity_constraint.end_distance = 10.0;
+ {
+ frc971::Constraint::Builder velocity_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ velocity_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VELOCITY);
+ velocity_constraint_builder.add_value(4.5);
+ velocity_constraint_builder.add_start_distance(0.0);
+ velocity_constraint_builder.add_end_distance(10.0);
+ velocity_constraint_offset = velocity_constraint_builder.Finish();
+ }
- spline.spline_count = 1;
- spline.spline_x = {{6.6, 7.511, 6.332, 4.590, 1.561, 0.179}};
- spline.spline_y = {{3.391, 2.826, 1.384, 3.395 - 0.20, 3.429 - 0.20, 3.434 - 0.20}};
+ flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
+ constraints_offset =
+ builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
+ {longitudinal_constraint_offset, lateral_constraint_offset,
+ voltage_constraint_offset, velocity_constraint_offset});
- spline.constraints = {{longitudinal_constraint, lateral_constraint,
- voltage_constraint, velocity_constraint}};
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder->fbb()->CreateVector<float>(
+ {6.6, 7.511, 6.332, 4.590, 1.561, 0.179});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder->fbb()->CreateVector<float>(
+ {3.391, 2.826, 1.384, 3.395 - 0.20, 3.429 - 0.20, 3.434 - 0.20});
+ MaybeFlipSpline(builder, spline_y_offset, is_left);
- MaybeFlipSpline(&spline, is_left);
- return spline;
+ frc971::MultiSpline::Builder multispline_builder =
+ builder->MakeBuilder<frc971::MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_constraints(constraints_offset);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ return multispline_builder.Finish();
}
// Path from the human player station to the far side of the rocket with a panel
-::frc971::MultiSpline AutonomousSplines::HPToFarRocket(bool is_left) {
- ::frc971::MultiSpline spline;
- ::frc971::Constraint lateral_constraint;
- ::frc971::Constraint longitudinal_constraint;
- ::frc971::Constraint velocity_constraint;
- ::frc971::Constraint voltage_constraint;
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::HPToFarRocket(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ bool is_left) {
+ flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> velocity_constraint_offset;
- longitudinal_constraint.constraint_type = 1;
- longitudinal_constraint.value = 3.0;
+ {
+ frc971::Constraint::Builder longitudinal_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ longitudinal_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_LONGITUDINAL_ACCELERATION);
+ longitudinal_constraint_builder.add_value(3.0);
+ longitudinal_constraint_offset = longitudinal_constraint_builder.Finish();
+ }
- lateral_constraint.constraint_type = 2;
- lateral_constraint.value = 3.0;
+ {
+ frc971::Constraint::Builder lateral_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ lateral_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_LATERAL_ACCELERATION);
+ lateral_constraint_builder.add_value(3.0);
+ lateral_constraint_offset = lateral_constraint_builder.Finish();
+ }
- voltage_constraint.constraint_type = 3;
- voltage_constraint.value = 11.0;
+ {
+ frc971::Constraint::Builder voltage_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ voltage_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VOLTAGE);
+ voltage_constraint_builder.add_value(11.0);
+ voltage_constraint_offset = voltage_constraint_builder.Finish();
+ }
- velocity_constraint.constraint_type = 4;
- velocity_constraint.value = 2.0;
- velocity_constraint.start_distance = 7.0;
- velocity_constraint.end_distance = 15.0;
+ {
+ frc971::Constraint::Builder velocity_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ velocity_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VELOCITY);
+ velocity_constraint_builder.add_value(4.0);
+ velocity_constraint_builder.add_start_distance(7.0);
+ velocity_constraint_builder.add_end_distance(15.0);
+ velocity_constraint_offset = velocity_constraint_builder.Finish();
+ }
- spline.spline_count = 1;
- spline.spline_x = {{0.895115737979, 2.9155615909, 5.02361983866,
- 6.40346237218, 7.1260656844, 7.83907559509}};
- spline.spline_y = {{3.43030859063, 3.44230565037, 2.8824369646, 2.81000389973,
- 3.08853311072, 2.6933085577}};
+ flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
+ constraints_offset =
+ builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
+ {longitudinal_constraint_offset, lateral_constraint_offset,
+ voltage_constraint_offset, velocity_constraint_offset});
- spline.constraints = {{lateral_constraint, velocity_constraint,
- voltage_constraint, longitudinal_constraint}};
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder->fbb()->CreateVector<float>({0.895115737979, 2.9155615909,
+ 5.02361983866, 6.40346237218,
+ 7.1260656844, 7.83907559509});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder->fbb()->CreateVector<float>({3.43030859063, 3.44230565037,
+ 2.8824369646, 2.81000389973,
+ 3.08853311072, 2.6933085577});
+ MaybeFlipSpline(builder, spline_y_offset, is_left);
- MaybeFlipSpline(&spline, is_left);
- return spline;
+ frc971::MultiSpline::Builder multispline_builder =
+ builder->MakeBuilder<frc971::MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_constraints(constraints_offset);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ return multispline_builder.Finish();
}
// Path from the far side of the rocket to close to the loading station
-::frc971::MultiSpline AutonomousSplines::FarRocketToNearHP(bool is_left) {
- ::frc971::MultiSpline spline;
- ::frc971::Constraint constraints;
-
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::FarRocketToNearHP(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ bool is_left) {
// TODO(theo): Add some real constraints.
- constraints.constraint_type = 0;
- constraints.value = 0;
- constraints.start_distance = 0;
- constraints.end_distance = 0;
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder->fbb()->CreateVector<float>({6.51652191988, 6.83156293562,
+ 5.74513904409, 2.2337653586,
+ 1.94766705864, 0.727526876557});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder->fbb()->CreateVector<float>({3.2465107468, 2.88277456846,
+ 1.93458779243, 3.44064777429,
+ 3.44377880106, 3.43326367284});
+ MaybeFlipSpline(builder, spline_y_offset, is_left);
- spline.spline_count = 1;
- spline.spline_x = {{6.51652191988, 6.83156293562, 5.74513904409, 2.2337653586,
- 1.94766705864, 0.727526876557}};
- spline.spline_y = {{3.2465107468, 2.88277456846, 1.93458779243, 3.44064777429,
- 3.44377880106, 3.43326367284}};
+ frc971::MultiSpline::Builder multispline_builder =
+ builder->MakeBuilder<frc971::MultiSpline>();
- spline.constraints = {{constraints}};
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
- MaybeFlipSpline(&spline, is_left);
- return spline;
+ return multispline_builder.Finish();
}
// Path from level 2 to 2nd cargo ship bay with a hatch panel
-::frc971::MultiSpline AutonomousSplines::HABToSecondCargoShipBay(bool is_left) {
- ::frc971::MultiSpline spline;
- ::frc971::Constraint constraints;
- ::frc971::Constraint longitudinal_constraint;
- ::frc971::Constraint voltage_constraint;
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::HABToSecondCargoShipBay(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ bool is_left) {
+ flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> velocity_constraint_offset;
- constraints.constraint_type = 4;
- constraints.value = 1.6;
- constraints.start_distance = 4.0;
- constraints.end_distance = 10.0;
+ {
+ frc971::Constraint::Builder longitudinal_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ longitudinal_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_LONGITUDINAL_ACCELERATION);
+ longitudinal_constraint_builder.add_value(2.0);
+ longitudinal_constraint_offset = longitudinal_constraint_builder.Finish();
+ }
- longitudinal_constraint.constraint_type = 1;
- longitudinal_constraint.value = 2.0;
+ {
+ frc971::Constraint::Builder voltage_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ voltage_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VOLTAGE);
+ voltage_constraint_builder.add_value(10.0);
+ voltage_constraint_offset = voltage_constraint_builder.Finish();
+ }
- voltage_constraint.constraint_type = 3;
- voltage_constraint.value = 10.0;
+ {
+ frc971::Constraint::Builder velocity_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ velocity_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VELOCITY);
+ velocity_constraint_builder.add_value(1.6);
+ velocity_constraint_builder.add_start_distance(4.0);
+ velocity_constraint_builder.add_end_distance(10.0);
+ velocity_constraint_offset = velocity_constraint_builder.Finish();
+ }
- spline.spline_count = 1;
+ flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
+ constraints_offset =
+ builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
+ {longitudinal_constraint_offset, voltage_constraint_offset,
+ velocity_constraint_offset});
+
constexpr double kLess = 0.06;
- spline.spline_x = {{1.0, 2.53944573074, 5.75526086906, 6.52583747973 - kLess,
- 7.12318661548 - kLess, 7.22595029399 - kLess}};
- spline.spline_y = {{1.5, 1.48, 2.05178220103,
- 2.56666687655, 1.79340280288, 1.16170693058}};
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder->fbb()->CreateVector<float>(
+ {1.0, 2.53944573074, 5.75526086906, 6.52583747973 - kLess,
+ 7.12318661548 - kLess, 7.22595029399 - kLess});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder->fbb()->CreateVector<float>({1.5, 1.48, 2.05178220103,
+ 2.56666687655, 1.79340280288,
+ 1.16170693058});
+ MaybeFlipSpline(builder, spline_y_offset, is_left);
- spline.constraints = {{constraints, longitudinal_constraint, voltage_constraint}};
+ frc971::MultiSpline::Builder multispline_builder =
+ builder->MakeBuilder<frc971::MultiSpline>();
- MaybeFlipSpline(&spline, is_left);
- return spline;
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_constraints(constraints_offset);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ return multispline_builder.Finish();
}
// Path from 2nd cargo ship bay to loading station
-::frc971::MultiSpline AutonomousSplines::SecondCargoShipBayToHP(bool is_left) {
- ::frc971::MultiSpline spline;
- ::frc971::Constraint constraints;
- ::frc971::Constraint voltage_constraint;
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::SecondCargoShipBayToHP(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ bool is_left) {
+ flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> velocity_constraint_offset;
- constraints.constraint_type = 4;
- constraints.value = 4.0;
- constraints.start_distance = 0;
- constraints.end_distance = 10;
+ {
+ frc971::Constraint::Builder voltage_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ voltage_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VOLTAGE);
+ voltage_constraint_builder.add_value(11.0);
+ voltage_constraint_offset = voltage_constraint_builder.Finish();
+ }
- voltage_constraint.constraint_type = 3;
- voltage_constraint.value = 11.0;
+ {
+ frc971::Constraint::Builder velocity_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ velocity_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VELOCITY);
+ velocity_constraint_builder.add_value(4.0);
+ velocity_constraint_builder.add_start_distance(0.0);
+ velocity_constraint_builder.add_end_distance(10.0);
+ velocity_constraint_offset = velocity_constraint_builder.Finish();
+ }
- spline.spline_count = 1;
- spline.spline_x = {{7.22595029399, 7.1892447864, 6.5373977907, 5.55997590982,
- 1.22953437637, 0.32521840905}};
+ flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
+ constraints_offset =
+ builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
+ {voltage_constraint_offset, velocity_constraint_offset});
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder->fbb()->CreateVector<float>({7.22595029399, 7.1892447864,
+ 6.5373977907, 5.55997590982,
+ 1.22953437637, 0.32521840905});
constexpr double kYShift = 0.1;
- spline.spline_y = {{1.2, 1.44543230529, 2.00646674662,
- 3.43762336271 - kYShift, 3.44125430793 - kYShift,
- 3.4360348159 - kYShift}};
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder->fbb()->CreateVector<float>(
+ {1.2, 1.44543230529, 2.00646674662, 3.43762336271 - kYShift,
+ 3.44125430793 - kYShift, 3.4360348159 - kYShift});
+ MaybeFlipSpline(builder, spline_y_offset, is_left);
- spline.constraints = {{constraints, voltage_constraint}};
+ frc971::MultiSpline::Builder multispline_builder =
+ builder->MakeBuilder<frc971::MultiSpline>();
- MaybeFlipSpline(&spline, is_left);
- return spline;
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_constraints(constraints_offset);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ return multispline_builder.Finish();
}
// Path from loading station to 3rd cargo ship bay with a hatch panel
-::frc971::MultiSpline AutonomousSplines::HPToThirdCargoShipBay(bool is_left) {
- ::frc971::MultiSpline spline;
- ::frc971::Constraint velocity_constraint;
- ::frc971::Constraint voltage_constraint;
- ::frc971::Constraint velocity_constraint2;
- velocity_constraint.constraint_type = 4;
- velocity_constraint.value = 3.5;
- velocity_constraint.start_distance = 0;
- velocity_constraint.end_distance = 10;
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::HPToThirdCargoShipBay(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ bool is_left) {
+ flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> velocity_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> velocity_constraint2_offset;
- velocity_constraint2.constraint_type = 4;
- velocity_constraint2.value = 2.0;
- velocity_constraint2.start_distance = 6;
- velocity_constraint2.end_distance = 10;
+ {
+ frc971::Constraint::Builder voltage_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ voltage_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VOLTAGE);
+ voltage_constraint_builder.add_value(10.0);
+ voltage_constraint_offset = voltage_constraint_builder.Finish();
+ }
- voltage_constraint.constraint_type = 3;
- voltage_constraint.value = 10.0;
+ {
+ frc971::Constraint::Builder velocity_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ velocity_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VELOCITY);
+ velocity_constraint_builder.add_value(3.5);
+ velocity_constraint_builder.add_start_distance(0.0);
+ velocity_constraint_builder.add_end_distance(10.0);
+ velocity_constraint_offset = velocity_constraint_builder.Finish();
+ }
- spline.spline_count = 1;
+ {
+ frc971::Constraint::Builder velocity_constraint2_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ velocity_constraint2_builder.add_constraint_type(
+ frc971::ConstraintType_VELOCITY);
+ velocity_constraint2_builder.add_value(2.0);
+ velocity_constraint2_builder.add_start_distance(6.0);
+ velocity_constraint2_builder.add_end_distance(10.0);
+ velocity_constraint2_offset = velocity_constraint2_builder.Finish();
+ }
+
+ flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
+ constraints_offset =
+ builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
+ {voltage_constraint_offset, velocity_constraint_offset,
+ velocity_constraint2_offset});
+
constexpr double kEndMove = 0.25;
- spline.spline_x = {{0.75, 1.112, 5.576, 7.497 - kEndMove, 7.675 - kEndMove,
- 7.768 - kEndMove}};
- spline.spline_y = {{3.431, 3.434, 2.712, 2.874, 1.786, 1.168}};
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder->fbb()->CreateVector<float>({0.75, 1.112, 5.576, 7.497 - kEndMove,
+ 7.675 - kEndMove, 7.768 - kEndMove});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder->fbb()->CreateVector<float>(
+ {3.431, 3.434, 2.712, 2.874, 1.786, 1.168});
+ MaybeFlipSpline(builder, spline_y_offset, is_left);
- spline.constraints = {
- {velocity_constraint, voltage_constraint, velocity_constraint2}};
+ frc971::MultiSpline::Builder multispline_builder =
+ builder->MakeBuilder<frc971::MultiSpline>();
- MaybeFlipSpline(&spline, is_left);
- return spline;
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_constraints(constraints_offset);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ return multispline_builder.Finish();
}
// Path from 3rd cargo ship bay to near the loading station
-::frc971::MultiSpline AutonomousSplines::ThirdCargoShipBayToNearHP(
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::ThirdCargoShipBayToNearHP(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
bool is_left) {
- ::frc971::MultiSpline spline;
- ::frc971::Constraint velocity_constraint;
+ flatbuffers::Offset<frc971::Constraint> velocity_constraint_offset;
- velocity_constraint.constraint_type = 4;
- velocity_constraint.value = 0.5;
- velocity_constraint.start_distance = 0;
- velocity_constraint.end_distance = 10;
+ {
+ frc971::Constraint::Builder velocity_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ velocity_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VELOCITY);
+ velocity_constraint_builder.add_value(0.5);
+ velocity_constraint_builder.add_start_distance(0.0);
+ velocity_constraint_builder.add_end_distance(10.0);
+ velocity_constraint_offset = velocity_constraint_builder.Finish();
+ }
- spline.spline_count = 1;
- spline.spline_x = {{7.75823205276, 7.58356294646, 5.95536035287,
- 2.12377989323, 1.29347361128, 0.598613577531}};
- spline.spline_y = {{1.16791407107, 1.94564064915, 2.54565614767,
- 3.43728005786, 3.43775494434, 3.43119598027}};
+ flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
+ constraints_offset =
+ builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
+ {velocity_constraint_offset});
- spline.constraints = {{velocity_constraint}};
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder->fbb()->CreateVector<float>({7.75823205276, 7.58356294646,
+ 5.95536035287, 2.12377989323,
+ 1.29347361128, 0.598613577531});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder->fbb()->CreateVector<float>({1.16791407107, 1.94564064915,
+ 2.54565614767, 3.43728005786,
+ 3.43775494434, 3.43119598027});
+ MaybeFlipSpline(builder, spline_y_offset, is_left);
- MaybeFlipSpline(&spline, is_left);
- return spline;
+ frc971::MultiSpline::Builder multispline_builder =
+ builder->MakeBuilder<frc971::MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_constraints(constraints_offset);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ return multispline_builder.Finish();
}
-::frc971::MultiSpline AutonomousSplines::HabToFarRocketTest(bool is_left) {
- ::frc971::MultiSpline spline;
- ::frc971::Constraint longitudinal_constraint;
- ::frc971::Constraint lateral_constraint;
- ::frc971::Constraint voltage_constraint;
- ::frc971::Constraint velocity_constraint;
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::HabToFarRocketTest(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ bool is_left) {
+ flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> velocity_constraint_offset;
- longitudinal_constraint.constraint_type = 1;
- longitudinal_constraint.value = 2.0;
+ {
+ frc971::Constraint::Builder longitudinal_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ longitudinal_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_LONGITUDINAL_ACCELERATION);
+ longitudinal_constraint_builder.add_value(2.0);
+ longitudinal_constraint_offset = longitudinal_constraint_builder.Finish();
+ }
- lateral_constraint.constraint_type = 2;
- lateral_constraint.value = 2.0;
+ {
+ frc971::Constraint::Builder lateral_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ lateral_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_LATERAL_ACCELERATION);
+ lateral_constraint_builder.add_value(2.0);
+ lateral_constraint_offset = lateral_constraint_builder.Finish();
+ }
- voltage_constraint.constraint_type = 3;
- voltage_constraint.value = 11.0;
+ {
+ frc971::Constraint::Builder voltage_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ voltage_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VOLTAGE);
+ voltage_constraint_builder.add_value(11.0);
+ voltage_constraint_offset = voltage_constraint_builder.Finish();
+ }
- velocity_constraint.constraint_type = 4;
- velocity_constraint.value = 1.7;
- velocity_constraint.start_distance = 0.0;
- velocity_constraint.end_distance = 0.8;
+ {
+ frc971::Constraint::Builder velocity_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ velocity_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VELOCITY);
+ velocity_constraint_builder.add_value(1.7);
+ velocity_constraint_builder.add_start_distance(0.0);
+ velocity_constraint_builder.add_end_distance(0.8);
+ velocity_constraint_offset = velocity_constraint_builder.Finish();
+ }
- spline.spline_count = 1;
- spline.spline_x = {{1.14763818102, 2.53944573074, 3.74586892131,
- 5.22352745444, 6.70255737219, 7.35784750785}};
- spline.spline_y = {{1.30261224364, 1.28295363394, 1.27450357714,
- 2.89953366429, 3.10734391012, 2.90125929705}};
+ flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
+ constraints_offset =
+ builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
+ {longitudinal_constraint_offset, lateral_constraint_offset,
+ voltage_constraint_offset, velocity_constraint_offset});
- spline.constraints = {{longitudinal_constraint, lateral_constraint,
- voltage_constraint, velocity_constraint}};
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder->fbb()->CreateVector<float>({1.14763818102, 2.53944573074,
+ 3.74586892131, 5.22352745444,
+ 6.70255737219, 7.35784750785});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder->fbb()->CreateVector<float>({1.30261224364, 1.28295363394,
+ 1.27450357714, 2.89953366429,
+ 3.10734391012, 2.90125929705});
+ MaybeFlipSpline(builder, spline_y_offset, is_left);
- MaybeFlipSpline(&spline, is_left);
- return spline;
+ frc971::MultiSpline::Builder multispline_builder =
+ builder->MakeBuilder<frc971::MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_constraints(constraints_offset);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ return multispline_builder.Finish();
}
-::frc971::MultiSpline AutonomousSplines::FarRocketToHPTest() {
- ::frc971::MultiSpline spline;
- ::frc971::Constraint longitudinal_constraint;
- ::frc971::Constraint lateral_constraint;
- ::frc971::Constraint voltage_constraint;
- ::frc971::Constraint velocity_constraint;
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::FarRocketToHPTest(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+ flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> velocity_constraint_offset;
- longitudinal_constraint.constraint_type = 1;
- longitudinal_constraint.value = 1.5;
+ {
+ frc971::Constraint::Builder longitudinal_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ longitudinal_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_LONGITUDINAL_ACCELERATION);
+ longitudinal_constraint_builder.add_value(1.5);
+ longitudinal_constraint_offset = longitudinal_constraint_builder.Finish();
+ }
- lateral_constraint.constraint_type = 2;
- lateral_constraint.value = 1.0;
+ {
+ frc971::Constraint::Builder lateral_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ lateral_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_LATERAL_ACCELERATION);
+ lateral_constraint_builder.add_value(1.0);
+ lateral_constraint_offset = lateral_constraint_builder.Finish();
+ }
- voltage_constraint.constraint_type = 3;
- voltage_constraint.value = 11.0;
+ {
+ frc971::Constraint::Builder voltage_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ voltage_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VOLTAGE);
+ voltage_constraint_builder.add_value(11.0);
+ voltage_constraint_offset = voltage_constraint_builder.Finish();
+ }
- velocity_constraint.constraint_type = 4;
- velocity_constraint.value = 0.5;
- velocity_constraint.start_distance = 9.5;
- velocity_constraint.end_distance = 10.0;
+ {
+ frc971::Constraint::Builder velocity_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ velocity_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VELOCITY);
+ velocity_constraint_builder.add_value(0.5);
+ velocity_constraint_builder.add_start_distance(9.5);
+ velocity_constraint_builder.add_end_distance(10.0);
+ velocity_constraint_offset = velocity_constraint_builder.Finish();
+ }
- spline.spline_count = 1;
- spline.spline_x = {{6.53, 7.8, 7.8, 4.0, 2.0, 0.4}};
- spline.spline_y = {{3.47, 3.0, 1.5, 3.0, 3.4, 3.4}};
+ flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
+ constraints_offset =
+ builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
+ {longitudinal_constraint_offset, lateral_constraint_offset,
+ voltage_constraint_offset, velocity_constraint_offset});
- spline.constraints = {{longitudinal_constraint, lateral_constraint,
- voltage_constraint, velocity_constraint}};
- return spline;
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder->fbb()->CreateVector<float>({6.53, 7.8, 7.8, 4.0, 2.0, 0.4});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder->fbb()->CreateVector<float>({3.47, 3.0, 1.5, 3.0, 3.4, 3.4});
+
+ frc971::MultiSpline::Builder multispline_builder =
+ builder->MakeBuilder<frc971::MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_constraints(constraints_offset);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ return multispline_builder.Finish();
}
-::frc971::MultiSpline AutonomousSplines::HPToNearRocketTest() {
- ::frc971::MultiSpline spline;
- ::frc971::Constraint longitudinal_constraint;
- ::frc971::Constraint lateral_constraint;
- ::frc971::Constraint voltage_constraint;
- ::frc971::Constraint velocity_constraint;
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::HPToNearRocketTest(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+ flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> velocity_constraint_offset;
- longitudinal_constraint.constraint_type = 1;
- longitudinal_constraint.value = 1.0;
+ {
+ frc971::Constraint::Builder longitudinal_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ longitudinal_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_LONGITUDINAL_ACCELERATION);
+ longitudinal_constraint_builder.add_value(1.0);
+ longitudinal_constraint_offset = longitudinal_constraint_builder.Finish();
+ }
- lateral_constraint.constraint_type = 2;
- lateral_constraint.value = 1.0;
+ {
+ frc971::Constraint::Builder lateral_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ lateral_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_LATERAL_ACCELERATION);
+ lateral_constraint_builder.add_value(1.0);
+ lateral_constraint_offset = lateral_constraint_builder.Finish();
+ }
- voltage_constraint.constraint_type = 3;
- voltage_constraint.value = 11.0;
+ {
+ frc971::Constraint::Builder voltage_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ voltage_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VOLTAGE);
+ voltage_constraint_builder.add_value(11.0);
+ voltage_constraint_offset = voltage_constraint_builder.Finish();
+ }
- velocity_constraint.constraint_type = 4;
- velocity_constraint.value = 0.5;
- velocity_constraint.start_distance = 2.7;
- velocity_constraint.end_distance = 10.0;
+ {
+ frc971::Constraint::Builder velocity_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ velocity_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VELOCITY);
+ velocity_constraint_builder.add_value(0.5);
+ velocity_constraint_builder.add_start_distance(2.7);
+ velocity_constraint_builder.add_end_distance(10.0);
+ velocity_constraint_offset = velocity_constraint_builder.Finish();
+ }
- spline.spline_count = 1;
- spline.spline_x = {{1.5, 2.0, 3.0, 4.0, 4.5, 5.12}};
- spline.spline_y = {{3.4, 3.4, 3.4, 3.0, 3.0, 3.43}};
+ flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
+ constraints_offset =
+ builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
+ {longitudinal_constraint_offset, lateral_constraint_offset,
+ voltage_constraint_offset, velocity_constraint_offset});
- spline.constraints = {{longitudinal_constraint, lateral_constraint,
- voltage_constraint, velocity_constraint}};
- return spline;
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder->fbb()->CreateVector<float>({1.5, 2.0, 3.0, 4.0, 4.5, 5.12});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder->fbb()->CreateVector<float>({3.4, 3.4, 3.4, 3.0, 3.0, 3.43});
+
+ frc971::MultiSpline::Builder multispline_builder =
+ builder->MakeBuilder<frc971::MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_constraints(constraints_offset);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ return multispline_builder.Finish();
}
-::frc971::MultiSpline AutonomousSplines::BasicSSpline() {
- ::frc971::MultiSpline spline;
- ::frc971::Constraint longitudinal_constraint;
- ::frc971::Constraint lateral_constraint;
- ::frc971::Constraint voltage_constraint;
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::BasicSSpline(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+ flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
- longitudinal_constraint.constraint_type = 1;
- longitudinal_constraint.value = 1.0;
+ {
+ frc971::Constraint::Builder longitudinal_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ longitudinal_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_LONGITUDINAL_ACCELERATION);
+ longitudinal_constraint_builder.add_value(1.0);
+ longitudinal_constraint_offset = longitudinal_constraint_builder.Finish();
+ }
- lateral_constraint.constraint_type = 2;
- lateral_constraint.value = 1.0;
+ {
+ frc971::Constraint::Builder lateral_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ lateral_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_LATERAL_ACCELERATION);
+ lateral_constraint_builder.add_value(1.0);
+ lateral_constraint_offset = lateral_constraint_builder.Finish();
+ }
- voltage_constraint.constraint_type = 3;
- voltage_constraint.value = 6.0;
+ {
+ frc971::Constraint::Builder voltage_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ voltage_constraint_builder.add_constraint_type(
+ frc971::ConstraintType_VOLTAGE);
+ voltage_constraint_builder.add_value(6.0);
+ voltage_constraint_offset = voltage_constraint_builder.Finish();
+ }
- spline.spline_count = 1;
+ flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
+ constraints_offset =
+ builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
+ {longitudinal_constraint_offset, lateral_constraint_offset,
+ voltage_constraint_offset});
+
const float startx = 0.4;
const float starty = 3.4;
- spline.spline_x = {{0.0f + startx, 0.6f + startx, 0.6f + startx,
- 0.4f + startx, 0.4f + startx, 1.0f + startx}};
- spline.spline_y = {{starty - 0.0f, starty - 0.0f, starty - 0.3f,
- starty - 0.7f, starty - 1.0f, starty - 1.0f}};
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder->fbb()->CreateVector<float>({0.0f + startx, 0.6f + startx,
+ 0.6f + startx, 0.4f + startx,
+ 0.4f + startx, 1.0f + startx});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder->fbb()->CreateVector<float>({starty - 0.0f, starty - 0.0f,
+ starty - 0.3f, starty - 0.7f,
+ starty - 1.0f, starty - 1.0f});
- spline.constraints = {
- {longitudinal_constraint, lateral_constraint, voltage_constraint}};
+ frc971::MultiSpline::Builder multispline_builder =
+ builder->MakeBuilder<frc971::MultiSpline>();
- return spline;
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_constraints(constraints_offset);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ return multispline_builder.Finish();
}
-::frc971::MultiSpline AutonomousSplines::StraightLine() {
- ::frc971::MultiSpline spline;
- ::frc971::Constraint contraints;
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::StraightLine(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder->fbb()->CreateVector<float>(
+ {-12.3, -11.9, -11.5, -11.1, -10.6, -10.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder->fbb()->CreateVector<float>({1.25, 1.25, 1.25, 1.25, 1.25, 1.25});
- contraints.constraint_type = 0;
- contraints.value = 0.0;
- contraints.start_distance = 0.0;
- contraints.end_distance = 0.0;
+ frc971::MultiSpline::Builder multispline_builder =
+ builder->MakeBuilder<frc971::MultiSpline>();
- spline.spline_count = 1;
- spline.spline_x = {{-12.3, -11.9, -11.5, -11.1, -10.6, -10.0}};
- spline.spline_y = {{1.25, 1.25, 1.25, 1.25, 1.25, 1.25}};
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
- spline.constraints = {{contraints}};
-
- return spline;
+ return multispline_builder.Finish();
}
} // namespace actors
diff --git a/y2019/actors/auto_splines.h b/y2019/actors/auto_splines.h
index a55c2f1..5d79ba9 100644
--- a/y2019/actors/auto_splines.h
+++ b/y2019/actors/auto_splines.h
@@ -1,7 +1,9 @@
#ifndef Y2019_ACTORS_AUTO_SPLINES_H_
#define Y2019_ACTORS_AUTO_SPLINES_H_
-#include "frc971/control_loops/control_loops.q.h"
+#include "aos/events/event_loop.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
/*
The cooridinate system for the autonomous splines is the same as the spline
@@ -17,38 +19,60 @@
// Splines for 2 Panels on the far side of the Rocket
// Path off of level 2 to the far side of the rocket with a panel
- static ::frc971::MultiSpline HABToFarRocket(bool is_left);
+ static flatbuffers::Offset<frc971::MultiSpline> HABToFarRocket(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ bool is_left);
// Path from the far side of the rocket to the loading station to pickup
- static ::frc971::MultiSpline FarRocketToHP(bool is_left);
+ static flatbuffers::Offset<frc971::MultiSpline> FarRocketToHP(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ bool is_left);
// Path from the far side of the rocket to the loading station to pickup
- static ::frc971::MultiSpline HPToFarRocket(bool is_left);
+ static flatbuffers::Offset<frc971::MultiSpline> HPToFarRocket(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ bool is_left);
// Path from the far side of the rocket to close to the loading station
- static ::frc971::MultiSpline FarRocketToNearHP(bool is_left);
+ static flatbuffers::Offset<frc971::MultiSpline> FarRocketToNearHP(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ bool is_left);
// Splines for 2 Panels on the far reaches of the cargo ship
// Path from level 2 to 2nd cargo ship bay with a hatch panel
- static ::frc971::MultiSpline HABToSecondCargoShipBay(bool is_left);
+ static flatbuffers::Offset<frc971::MultiSpline> HABToSecondCargoShipBay(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ bool is_left);
// Path from 2nd cargo ship bay to loading station
- static ::frc971::MultiSpline SecondCargoShipBayToHP(bool is_left);
+ static flatbuffers::Offset<frc971::MultiSpline> SecondCargoShipBayToHP(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ bool is_left);
// Path from loading station to 3rd cargo ship bay with a hatch panel
- static ::frc971::MultiSpline HPToThirdCargoShipBay(bool is_left);
+ static flatbuffers::Offset<frc971::MultiSpline> HPToThirdCargoShipBay(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ bool is_left);
// Path from 3rd cargo ship bay to near the loading station
- static ::frc971::MultiSpline ThirdCargoShipBayToNearHP(bool is_left);
+ static flatbuffers::Offset<frc971::MultiSpline> ThirdCargoShipBayToNearHP(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ bool is_left);
// Testing Splines
- static ::frc971::MultiSpline HPToNearRocketTest();
- static ::frc971::MultiSpline HabToFarRocketTest(bool is_left);
- static ::frc971::MultiSpline FarRocketToHPTest();
+ static flatbuffers::Offset<frc971::MultiSpline> HPToNearRocketTest(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder);
+ static flatbuffers::Offset<frc971::MultiSpline> HabToFarRocketTest(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+ bool is_left);
+ static flatbuffers::Offset<frc971::MultiSpline> FarRocketToHPTest(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder);
- static ::frc971::MultiSpline BasicSSpline();
- static ::frc971::MultiSpline StraightLine();
+ static flatbuffers::Offset<frc971::MultiSpline> BasicSSpline(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder);
+ static flatbuffers::Offset<frc971::MultiSpline> StraightLine(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder);
};
} // namespace actors
diff --git a/y2019/actors/autonomous_actor.cc b/y2019/actors/autonomous_actor.cc
index a5a71aa..fbde0be 100644
--- a/y2019/actors/autonomous_actor.cc
+++ b/y2019/actors/autonomous_actor.cc
@@ -8,14 +8,16 @@
#include "aos/logging/logging.h"
#include "aos/util/phased_loop.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/control_loops/drivetrain/localizer.q.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "y2019/actors/auto_splines.h"
#include "y2019/control_loops/drivetrain/drivetrain_base.h"
namespace y2019 {
namespace actors {
+
+using ::frc971::ProfileParametersT;
using ::aos::monotonic_clock;
+using frc971::control_loops::drivetrain::LocalizerControl;
namespace chrono = ::std::chrono;
AutonomousActor::AutonomousActor(::aos::EventLoop *event_loop)
@@ -24,15 +26,14 @@
localizer_control_sender_(
event_loop->MakeSender<
::frc971::control_loops::drivetrain::LocalizerControl>(
- ".frc971.control_loops.drivetrain.localizer_control")),
+ "/drivetrain")),
superstructure_goal_sender_(
- event_loop->MakeSender<::y2019::control_loops::superstructure::
- SuperstructureQueue::Goal>(
- ".y2019.control_loops.superstructure.superstructure_queue.goal")),
- superstructure_status_fetcher_(event_loop->MakeFetcher<
- ::y2019::control_loops::superstructure::
- SuperstructureQueue::Status>(
- ".y2019.control_loops.superstructure.superstructure_queue.status")) {}
+ event_loop->MakeSender<::y2019::control_loops::superstructure::Goal>(
+ "/superstructure")),
+ superstructure_status_fetcher_(
+ event_loop
+ ->MakeFetcher<::y2019::control_loops::superstructure::Status>(
+ "/superstructure")) {}
bool AutonomousActor::WaitForDriveXGreater(double x) {
AOS_LOG(INFO, "Waiting until x > %f\n", x);
@@ -46,8 +47,8 @@
}
phased_loop.SleepUntilNext();
drivetrain_status_fetcher_.Fetch();
- if (drivetrain_status_fetcher_->x > x) {
- AOS_LOG(INFO, "X at %f\n", drivetrain_status_fetcher_->x);
+ if (drivetrain_status_fetcher_->x() > x) {
+ AOS_LOG(INFO, "X at %f\n", drivetrain_status_fetcher_->x());
return true;
}
}
@@ -65,8 +66,8 @@
}
phased_loop.SleepUntilNext();
drivetrain_status_fetcher_.Fetch();
- if (::std::abs(drivetrain_status_fetcher_->y) < y) {
- AOS_LOG(INFO, "Y at %f\n", drivetrain_status_fetcher_->y);
+ if (::std::abs(drivetrain_status_fetcher_->y()) < y) {
+ AOS_LOG(INFO, "Y at %f\n", drivetrain_status_fetcher_->y());
return true;
}
}
@@ -90,13 +91,16 @@
SendSuperstructureGoal();
{
- auto localizer_resetter = localizer_control_sender_.MakeMessage();
+ auto builder = localizer_control_sender_.MakeBuilder();
+
+ LocalizerControl::Builder localizer_control_builder =
+ builder.MakeBuilder<LocalizerControl>();
// Start on the left l2.
- localizer_resetter->x = 1.0;
- localizer_resetter->y = 1.35 * turn_scalar;
- localizer_resetter->theta = M_PI;
- localizer_resetter->theta_uncertainty = 0.00001;
- if (!localizer_resetter.Send()) {
+ localizer_control_builder.add_x(1.0);
+ localizer_control_builder.add_y(1.35 * turn_scalar);
+ localizer_control_builder.add_theta(M_PI);
+ localizer_control_builder.add_theta_uncertainty(0.00001);
+ if (!builder.Send(localizer_control_builder.Finish())) {
AOS_LOG(ERROR, "Failed to reset localizer.\n");
}
}
@@ -105,20 +109,28 @@
// Otherwise our drivetrain reset will do a 180 right at the start.
WaitUntil([this]() { return drivetrain_status_fetcher_.Fetch(); });
AOS_LOG(INFO, "Heading is %f\n",
- drivetrain_status_fetcher_->estimated_heading);
+ drivetrain_status_fetcher_->estimated_heading());
InitializeEncoders();
ResetDrivetrain();
WaitUntil([this]() { return drivetrain_status_fetcher_.Fetch(); });
AOS_LOG(INFO, "Heading is %f\n",
- drivetrain_status_fetcher_->estimated_heading);
+ drivetrain_status_fetcher_->estimated_heading());
ResetDrivetrain();
InitializeEncoders();
}
-const ProfileParameters kJumpDrive = {2.0, 3.0};
-const ProfileParameters kDrive = {4.0, 3.0};
-const ProfileParameters kTurn = {5.0, 15.0};
+ProfileParametersT MakeProfileParameters(float max_velocity,
+ float max_acceleration) {
+ ProfileParametersT result;
+ result.max_velocity = max_velocity;
+ result.max_acceleration = max_acceleration;
+ return result;
+}
+
+const ProfileParametersT kJumpDrive = MakeProfileParameters(2.0, 3.0);
+const ProfileParametersT kDrive = MakeProfileParameters(4.0, 3.0);
+const ProfileParametersT kTurn = MakeProfileParameters(5.0, 15.0);
const ElevatorWristPosition kPanelHPIntakeForwrdPos{0.01, M_PI / 2.0};
const ElevatorWristPosition kPanelHPIntakeBackwardPos{0.015, -M_PI / 2.0};
@@ -130,14 +142,23 @@
const ElevatorWristPosition kPanelCargoBackwardPos{0.0, -M_PI / 2.0};
+template <typename Functor>
+std::function<flatbuffers::Offset<frc971::MultiSpline>(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder)>
+BindIsLeft(Functor f, bool is_left) {
+ return
+ [is_left, f](aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder
+ *builder) { return f(builder, is_left); };
+}
+
bool AutonomousActor::RunAction(
- const ::frc971::autonomous::AutonomousActionParams ¶ms) {
+ const ::frc971::autonomous::AutonomousActionParams *params) {
const monotonic_clock::time_point start_time = monotonic_now();
- const bool is_left = params.mode == 0;
+ const bool is_left = params->mode() == 0;
{
AOS_LOG(INFO, "Starting autonomous action with mode %" PRId32 " %s\n",
- params.mode, is_left ? "left" : "right");
+ params->mode(), is_left ? "left" : "right");
}
const double turn_scalar = is_left ? 1.0 : -1.0;
@@ -147,7 +168,7 @@
Mode mode = Mode::kCargoship;
if (mode == Mode::kRocket) {
SplineHandle spline1 =
- PlanSpline(AutonomousSplines::HabToFarRocketTest(is_left),
+ PlanSpline(BindIsLeft(AutonomousSplines::HabToFarRocketTest, is_left),
SplineDirection::kBackward);
// Grab the disk, jump, wait until we have vacuum, then raise the elevator
@@ -176,15 +197,18 @@
// END SPLINE 1
if (!spline1.WaitForSplineDistanceRemaining(0.2)) return true;
- LineFollowAtVelocity(1.3, 4);
+ LineFollowAtVelocity(1.3,
+ control_loops::drivetrain::SelectionHint_FAR_ROCKET);
if (!WaitForMilliseconds(::std::chrono::milliseconds(1200))) return true;
set_suction_goal(false, 1);
SendSuperstructureGoal();
if (!WaitForMilliseconds(::std::chrono::milliseconds(200))) return true;
- LineFollowAtVelocity(-1.0, 4);
- SplineHandle spline2 = PlanSpline(AutonomousSplines::FarRocketToHP(is_left),
- SplineDirection::kBackward);
+ LineFollowAtVelocity(-1.0,
+ control_loops::drivetrain::SelectionHint_FAR_ROCKET);
+ SplineHandle spline2 =
+ PlanSpline(BindIsLeft(AutonomousSplines::FarRocketToHP, is_left),
+ SplineDirection::kBackward);
if (!WaitForMilliseconds(::std::chrono::milliseconds(150))) return true;
if (!spline2.WaitForPlan()) return true;
@@ -204,8 +228,9 @@
// As soon as we pick up Panel 2 go score on the back rocket
if (!WaitForGamePiece()) return true;
LineFollowAtVelocity(1.5);
- SplineHandle spline3 = PlanSpline(AutonomousSplines::HPToFarRocket(is_left),
- SplineDirection::kForward);
+ SplineHandle spline3 =
+ PlanSpline(BindIsLeft(AutonomousSplines::HPToFarRocket, is_left),
+ SplineDirection::kForward);
if (!WaitForDriveXGreater(0.50)) return true;
if (!spline3.WaitForPlan()) return true;
spline3.Start();
@@ -214,7 +239,8 @@
set_elevator_wrist_goal(kPanelBackwardMiddlePos);
SendSuperstructureGoal();
if (!WaitForDriveXGreater(7.1)) return true;
- LineFollowAtVelocity(-1.5, 4);
+ LineFollowAtVelocity(-1.5,
+ control_loops::drivetrain::SelectionHint_FAR_ROCKET);
if (!WaitForMilliseconds(::std::chrono::milliseconds(1000))) return true;
set_elevator_wrist_goal(kPanelBackwardUpperPos);
SendSuperstructureGoal();
@@ -222,13 +248,14 @@
set_suction_goal(false, 1);
SendSuperstructureGoal();
if (!WaitForMilliseconds(::std::chrono::milliseconds(400))) return true;
- LineFollowAtVelocity(1.0, 4);
+ LineFollowAtVelocity(1.0,
+ control_loops::drivetrain::SelectionHint_FAR_ROCKET);
SendSuperstructureGoal();
if (!WaitForMilliseconds(::std::chrono::milliseconds(200))) return true;
} else if (mode == Mode::kCargoship) {
- SplineHandle spline1 =
- PlanSpline(AutonomousSplines::HABToSecondCargoShipBay(is_left),
- SplineDirection::kBackward);
+ SplineHandle spline1 = PlanSpline(
+ BindIsLeft(AutonomousSplines::HABToSecondCargoShipBay, is_left),
+ SplineDirection::kBackward);
set_elevator_goal(0.01);
set_wrist_goal(-M_PI / 2.0);
set_intake_goal(-1.2);
@@ -254,7 +281,8 @@
if (!spline1.WaitForSplineDistanceRemaining(0.8)) return true;
// Line follow in to the first disc.
- LineFollowAtVelocity(-0.9, 2);
+ LineFollowAtVelocity(-0.9,
+ control_loops::drivetrain::SelectionHint_MID_SHIP);
if (!WaitForDriveYCloseToZero(1.2)) return true;
set_suction_goal(false, 1);
@@ -265,10 +293,11 @@
if (!WaitForDriveYCloseToZero(1.13)) return true;
if (!WaitForMilliseconds(::std::chrono::milliseconds(300))) return true;
- LineFollowAtVelocity(0.9, 2);
- SplineHandle spline2 =
- PlanSpline(AutonomousSplines::SecondCargoShipBayToHP(is_left),
- SplineDirection::kForward);
+ LineFollowAtVelocity(0.9,
+ control_loops::drivetrain::SelectionHint_MID_SHIP);
+ SplineHandle spline2 = PlanSpline(
+ BindIsLeft(AutonomousSplines::SecondCargoShipBayToHP, is_left),
+ SplineDirection::kForward);
if (!WaitForMilliseconds(::std::chrono::milliseconds(400))) return true;
if (!spline2.WaitForPlan()) return true;
AOS_LOG(INFO, "Planned\n");
@@ -288,9 +317,9 @@
AOS_LOG(INFO, "Got gamepiece %f\n",
::aos::time::DurationInSeconds(monotonic_now() - start_time));
LineFollowAtVelocity(-4.0);
- SplineHandle spline3 =
- PlanSpline(AutonomousSplines::HPToThirdCargoShipBay(is_left),
- SplineDirection::kBackward);
+ SplineHandle spline3 = PlanSpline(
+ BindIsLeft(AutonomousSplines::HPToThirdCargoShipBay, is_left),
+ SplineDirection::kBackward);
if (!WaitForDriveXGreater(0.55)) return true;
if (!spline3.WaitForPlan()) return true;
spline3.Start();
@@ -301,7 +330,8 @@
if (!spline3.WaitForSplineDistanceRemaining(0.7)) return true;
// Line follow in to the second disc.
- LineFollowAtVelocity(-0.7, 3);
+ LineFollowAtVelocity(-0.7,
+ control_loops::drivetrain::SelectionHint_FAR_SHIP);
AOS_LOG(INFO, "Drawing in disc 2 %f\n",
::aos::time::DurationInSeconds(monotonic_now() - start_time));
if (!WaitForDriveYCloseToZero(1.2)) return true;
@@ -315,7 +345,8 @@
if (!WaitForMilliseconds(::std::chrono::milliseconds(200))) return true;
AOS_LOG(INFO, "Backing up %f\n",
::aos::time::DurationInSeconds(monotonic_now() - start_time));
- LineFollowAtVelocity(0.9, 3);
+ LineFollowAtVelocity(0.9,
+ control_loops::drivetrain::SelectionHint_FAR_SHIP);
if (!WaitForMilliseconds(::std::chrono::milliseconds(400))) return true;
} else {
// Grab the disk, wait until we have vacuum, then jump
diff --git a/y2019/actors/autonomous_actor.h b/y2019/actors/autonomous_actor.h
index 8c61596..af2a60f 100644
--- a/y2019/actors/autonomous_actor.h
+++ b/y2019/actors/autonomous_actor.h
@@ -7,16 +7,18 @@
#include "aos/actions/actions.h"
#include "aos/actions/actor.h"
#include "frc971/autonomous/base_autonomous_actor.h"
-#include "frc971/control_loops/control_loops.q.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
-#include "frc971/control_loops/drivetrain/localizer.q.h"
-#include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_status_generated.h"
namespace y2019 {
namespace actors {
-using ::frc971::ProfileParameters;
+using ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
+
+namespace superstructure = y2019::control_loops::superstructure;
struct ElevatorWristPosition {
double elevator;
@@ -28,7 +30,7 @@
explicit AutonomousActor(::aos::EventLoop *event_loop);
bool RunAction(
- const ::frc971::autonomous::AutonomousActionParams ¶ms) override;
+ const ::frc971::autonomous::AutonomousActionParams *params) override;
private:
void Reset(bool is_left);
@@ -75,25 +77,83 @@
}
void SendSuperstructureGoal() {
- auto new_superstructure_goal = superstructure_goal_sender_.MakeMessage();
- new_superstructure_goal->elevator.unsafe_goal = elevator_goal_;
- new_superstructure_goal->wrist.unsafe_goal = wrist_goal_;
- new_superstructure_goal->intake.unsafe_goal = intake_goal_;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- new_superstructure_goal->suction.grab_piece = suction_on_;
- new_superstructure_goal->suction.gamepiece_mode = suction_gamepiece_;
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset;
- new_superstructure_goal->elevator.profile_params.max_velocity =
- elevator_max_velocity_;
- new_superstructure_goal->elevator.profile_params.max_acceleration =
- elevator_max_acceleration_;
+ {
+ frc971::ProfileParameters::Builder profile_params_builder =
+ builder.MakeBuilder<frc971::ProfileParameters>();
+ profile_params_builder.add_max_velocity(elevator_max_velocity_);
+ profile_params_builder.add_max_acceleration(elevator_max_acceleration_);
- new_superstructure_goal->wrist.profile_params.max_velocity =
- wrist_max_velocity_;
- new_superstructure_goal->wrist.profile_params.max_acceleration =
- wrist_max_acceleration_;
+ flatbuffers::Offset<frc971::ProfileParameters> profile_params_offset =
+ profile_params_builder.Finish();
- if (!new_superstructure_goal.Send()) {
+ StaticZeroingSingleDOFProfiledSubsystemGoal::Builder elevator_builder =
+ builder.MakeBuilder<StaticZeroingSingleDOFProfiledSubsystemGoal>();
+
+ elevator_builder.add_unsafe_goal(elevator_goal_);
+ elevator_builder.add_profile_params(profile_params_offset);
+
+ elevator_offset = elevator_builder.Finish();
+ }
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset;
+
+ {
+ frc971::ProfileParameters::Builder profile_params_builder =
+ builder.MakeBuilder<frc971::ProfileParameters>();
+ profile_params_builder.add_max_velocity(wrist_max_velocity_);
+ profile_params_builder.add_max_acceleration(wrist_max_acceleration_);
+
+ flatbuffers::Offset<frc971::ProfileParameters> profile_params_offset =
+ profile_params_builder.Finish();
+
+ StaticZeroingSingleDOFProfiledSubsystemGoal::Builder wrist_builder =
+ builder.MakeBuilder<StaticZeroingSingleDOFProfiledSubsystemGoal>();
+
+ wrist_builder.add_unsafe_goal(wrist_goal_);
+ wrist_builder.add_profile_params(profile_params_offset);
+
+ wrist_offset = wrist_builder.Finish();
+ }
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset;
+
+ {
+ StaticZeroingSingleDOFProfiledSubsystemGoal::Builder intake_builder =
+ builder.MakeBuilder<StaticZeroingSingleDOFProfiledSubsystemGoal>();
+
+ intake_builder.add_unsafe_goal(intake_goal_);
+
+ intake_offset = intake_builder.Finish();
+ }
+
+ flatbuffers::Offset<superstructure::SuctionGoal> suction_offset;
+
+ {
+ superstructure::SuctionGoal::Builder suction_builder =
+ builder.MakeBuilder<superstructure::SuctionGoal>();
+
+ suction_builder.add_grab_piece(suction_on_);
+ suction_builder.add_gamepiece_mode(suction_gamepiece_);
+
+ suction_offset = suction_builder.Finish();
+ }
+
+ superstructure::Goal::Builder superstructure_builder =
+ builder.MakeBuilder<superstructure::Goal>();
+
+ superstructure_builder.add_elevator(elevator_offset);
+ superstructure_builder.add_wrist(wrist_offset);
+ superstructure_builder.add_intake(intake_offset);
+ superstructure_builder.add_suction(suction_offset);
+
+ if (!builder.Send(superstructure_builder.Finish())) {
AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
}
}
@@ -102,7 +162,7 @@
superstructure_status_fetcher_.Fetch();
if (superstructure_status_fetcher_.get()) {
- return superstructure_status_fetcher_->has_piece;
+ return superstructure_status_fetcher_->has_piece();
}
return false;
}
@@ -145,12 +205,12 @@
if (superstructure_status_fetcher_.get()) {
const bool elevator_at_goal =
::std::abs(elevator_goal_ -
- superstructure_status_fetcher_->elevator.position) <
+ superstructure_status_fetcher_->elevator()->position()) <
kElevatorTolerance;
const bool wrist_at_goal =
::std::abs(wrist_goal_ -
- superstructure_status_fetcher_->wrist.position) <
+ superstructure_status_fetcher_->wrist()->position()) <
kWristTolerance;
return elevator_at_goal && wrist_at_goal;
@@ -183,11 +243,9 @@
::aos::Sender<::frc971::control_loops::drivetrain::LocalizerControl>
localizer_control_sender_;
- ::aos::Sender<
- ::y2019::control_loops::superstructure::SuperstructureQueue::Goal>
+ ::aos::Sender<::y2019::control_loops::superstructure::Goal>
superstructure_goal_sender_;
- ::aos::Fetcher<
- ::y2019::control_loops::superstructure::SuperstructureQueue::Status>
+ ::aos::Fetcher<::y2019::control_loops::superstructure::Status>
superstructure_status_fetcher_;
};
diff --git a/y2019/actors/autonomous_actor_main.cc b/y2019/actors/autonomous_actor_main.cc
index 79b52da..aa176f9 100644
--- a/y2019/actors/autonomous_actor_main.cc
+++ b/y2019/actors/autonomous_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 "frc971/autonomous/auto.q.h"
#include "y2019/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());
::y2019::actors::AutonomousActor autonomous(&event_loop);
event_loop.Run();
diff --git a/y2019/control_loops/drivetrain/BUILD b/y2019/control_loops/drivetrain/BUILD
index 9b9ba30..1575584 100644
--- a/y2019/control_loops/drivetrain/BUILD
+++ b/y2019/control_loops/drivetrain/BUILD
@@ -1,5 +1,5 @@
-load("//aos/build:queues.bzl", "queue_library")
-load("//tools/build_rules:select.bzl", "cpu_select", "compiler_select")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//tools/build_rules:select.bzl", "compiler_select", "cpu_select")
genrule(
name = "genrule_drivetrain",
@@ -31,39 +31,6 @@
],
)
-cc_binary(
- name = "replay_localizer",
- srcs = [
- "replay_localizer.cc",
- ],
- defines =
- cpu_select({
- "amd64": [
- "SUPPORT_PLOT=1",
- ],
- "arm": [],
- }),
- linkstatic = True,
- deps = [
- "//frc971/control_loops/drivetrain:localizer_queue",
- ":localizer",
- ":event_loop_localizer",
- ":drivetrain_base",
- "@com_github_gflags_gflags//:gflags",
- "//y2019:constants",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
- "//aos:init",
- "//aos/controls:replay_control_loop",
- "//frc971/queues:gyro",
- "//frc971/wpilib:imu_queue",
- ] + cpu_select({
- "amd64": [
- "//third_party/matplotlib-cpp",
- ],
- "arm": [],
- }),
-)
-
cc_library(
name = "polydrivetrain_plants",
srcs = [
@@ -111,24 +78,24 @@
":drivetrain_base",
":event_loop_localizer",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
"//frc971/control_loops/drivetrain:drivetrain_lib",
],
)
-queue_library(
- name = "target_selector_queue",
- srcs = [
- "target_selector.q",
- ],
+flatbuffer_cc_library(
+ name = "target_selector_fbs",
+ srcs = ["target_selector.fbs"],
+ gen_reflections = 1,
visibility = ["//visibility:public"],
)
-queue_library(
- name = "camera_queue",
+flatbuffer_cc_library(
+ name = "camera_fbs",
srcs = [
- "camera.q",
+ "camera.fbs",
],
+ gen_reflections = 1,
visibility = ["//visibility:public"],
)
@@ -167,17 +134,18 @@
hdrs = ["target_selector.h"],
deps = [
":camera",
- ":target_selector_queue",
+ ":target_selector_fbs",
"//frc971/control_loops:pose",
"//frc971/control_loops/drivetrain:localizer",
"//y2019:constants",
- "//y2019/control_loops/superstructure:superstructure_queue",
+ "//y2019/control_loops/superstructure:superstructure_goal_fbs",
],
)
cc_test(
name = "target_selector_test",
srcs = ["target_selector_test.cc"],
+ data = ["//y2019:config.json"],
deps = [
":target_selector",
"//aos/events:simulated_event_loop",
@@ -191,7 +159,7 @@
srcs = ["event_loop_localizer.cc"],
hdrs = ["event_loop_localizer.h"],
deps = [
- ":camera_queue",
+ ":camera_fbs",
":localizer",
":target_selector",
"//frc971/control_loops/drivetrain:localizer",
@@ -232,13 +200,14 @@
cc_test(
name = "localized_drivetrain_test",
srcs = ["localized_drivetrain_test.cc"],
+ data = ["//y2019:config.json"],
deps = [
- ":camera_queue",
+ ":camera_fbs",
":drivetrain_base",
":event_loop_localizer",
":localizer",
"//aos/controls:control_loop_test",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
"//aos/network:team_number",
"//frc971/control_loops:team_number_test_environment",
"//frc971/control_loops/drivetrain:drivetrain_lib",
diff --git a/y2019/control_loops/drivetrain/camera.fbs b/y2019/control_loops/drivetrain/camera.fbs
new file mode 100644
index 0000000..154dd2e
--- /dev/null
+++ b/y2019/control_loops/drivetrain/camera.fbs
@@ -0,0 +1,24 @@
+namespace y2019.control_loops.drivetrain;
+
+// See the Target structure in //y2019/jevois:structures.h for documentation.
+table CameraTarget {
+ distance:float;
+ height:float;
+ heading:float;
+ skew:float;
+}
+
+// Frames from a camera.
+table CameraFrame {
+ // Number of nanoseconds since the aos::monotonic_clock epoch at which this
+ // frame was captured.
+ timestamp:long;
+
+ // Buffer for the 3 targets.
+ targets:[CameraTarget];
+
+ // Index of the camera position (not serial number) which this frame is from.
+ camera:ubyte;
+}
+
+root_type CameraFrame;
diff --git a/y2019/control_loops/drivetrain/camera.q b/y2019/control_loops/drivetrain/camera.q
deleted file mode 100644
index 9d5dffc..0000000
--- a/y2019/control_loops/drivetrain/camera.q
+++ /dev/null
@@ -1,26 +0,0 @@
-package y2019.control_loops.drivetrain;
-
-// See the Target structure in //y2019/jevois:structures.h for documentation.
-struct CameraTarget {
- float distance;
- float height;
- float heading;
- float skew;
-};
-
-// Frames from a camera.
-// Published on ".y2019.control_loops.drivetrain.camera_frames"
-message CameraFrame {
- // Number of nanoseconds since the aos::monotonic_clock epoch at which this
- // frame was captured.
- int64_t timestamp;
-
- // Number of targets actually in this frame.
- uint8_t num_targets;
-
- // Buffer for the targets.
- CameraTarget[3] targets;
-
- // Index of the camera position (not serial number) which this frame is from.
- uint8_t camera;
-};
diff --git a/y2019/control_loops/drivetrain/drivetrain_main.cc b/y2019/control_loops/drivetrain/drivetrain_main.cc
index ada8f53..77df69b 100644
--- a/y2019/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2019/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 "y2019/control_loops/drivetrain/drivetrain_base.h"
#include "y2019/control_loops/drivetrain/event_loop_localizer.h"
@@ -10,7 +10,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());
::y2019::control_loops::drivetrain::EventLoopLocalizer localizer(
&event_loop, ::y2019::control_loops::drivetrain::GetDrivetrainConfig());
DrivetrainLoop drivetrain(
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.cc b/y2019/control_loops/drivetrain/event_loop_localizer.cc
index 67a708a..a372df4 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.cc
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.cc
@@ -45,8 +45,7 @@
localizer_.ResetInitialState(monotonic_now, localizer_.X_hat(),
localizer_.P());
});
- frame_fetcher_ = event_loop_->MakeFetcher<CameraFrame>(
- ".y2019.control_loops.drivetrain.camera_frames");
+ frame_fetcher_ = event_loop_->MakeFetcher<CameraFrame>("/drivetrain");
}
void EventLoopLocalizer::Reset(::aos::monotonic_clock::time_point now,
@@ -71,37 +70,39 @@
localizer_.UpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate, U,
now);
while (frame_fetcher_.FetchNext()) {
- HandleFrame(*frame_fetcher_.get());
+ HandleFrame(frame_fetcher_.get());
}
}
-void EventLoopLocalizer::HandleFrame(const CameraFrame &frame) {
+void EventLoopLocalizer::HandleFrame(const CameraFrame *frame) {
// We need to construct TargetView's and pass them to the localizer:
::aos::SizedArray<TargetView, kMaxTargetsPerFrame> views;
// Note: num_targets and camera are unsigned integers and so don't need to be
// checked for < 0.
- if (frame.num_targets > kMaxTargetsPerFrame) {
- AOS_LOG(ERROR, "Got bad num_targets %d\n", frame.num_targets);
+ size_t camera = frame->camera();
+ if (!frame->has_targets() || frame->targets()->size() > kMaxTargetsPerFrame) {
+ AOS_LOG(ERROR, "Got bad num_targets %d\n",
+ frame->has_targets() ? frame->targets()->size() : 0);
return;
}
- if (frame.camera > cameras_.size()) {
- AOS_LOG(ERROR, "Got bad camera number %d\n", frame.camera);
+ if (camera > cameras_.size()) {
+ AOS_LOG(ERROR, "Got bad camera number %zu\n", camera);
return;
}
- for (int ii = 0; ii < frame.num_targets; ++ii) {
+ for (const CameraTarget *target : *frame->targets()) {
TargetView view;
- view.reading.heading = frame.targets[ii].heading;
- view.reading.distance = frame.targets[ii].distance;
- view.reading.skew = frame.targets[ii].skew;
- view.reading.height = frame.targets[ii].height;
+ view.reading.heading = target->heading();
+ view.reading.distance = target->distance();
+ view.reading.skew = target->skew();
+ view.reading.height = target->height();
if (view.reading.distance < 2.25) {
- cameras_[frame.camera].PopulateNoise(&view);
+ cameras_[camera].PopulateNoise(&view);
views.push_back(view);
}
}
::aos::monotonic_clock::time_point t(
- ::std::chrono::nanoseconds(frame.timestamp));
- localizer_.UpdateTargets(cameras_[frame.camera], views, t);
+ ::std::chrono::nanoseconds(frame->timestamp()));
+ localizer_.UpdateTargets(cameras_[camera], views, t);
}
} // namespace drivetrain
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.h b/y2019/control_loops/drivetrain/event_loop_localizer.h
index dac614b..83b8e79 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.h
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.h
@@ -3,7 +3,7 @@
#include "frc971/control_loops/drivetrain/localizer.h"
#include "y2019/constants.h"
-#include "y2019/control_loops/drivetrain/camera.q.h"
+#include "y2019/control_loops/drivetrain/camera_generated.h"
#include "y2019/control_loops/drivetrain/localizer.h"
#include "y2019/control_loops/drivetrain/target_selector.h"
@@ -92,7 +92,7 @@
}
private:
- void HandleFrame(const CameraFrame &frame);
+ void HandleFrame(const CameraFrame *frame);
::aos::EventLoop *event_loop_;
// TODO(james): Make this use Watchers once we have them working in our
diff --git a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
index ad7223e..06d2f24 100644
--- a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
+++ b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
@@ -7,9 +7,9 @@
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
#include "frc971/control_loops/team_number_test_environment.h"
-#include "y2019/control_loops/drivetrain/camera.q.h"
-#include "y2019/control_loops/drivetrain/event_loop_localizer.h"
+#include "y2019/control_loops/drivetrain/camera_generated.h"
#include "y2019/control_loops/drivetrain/drivetrain_base.h"
+#include "y2019/control_loops/drivetrain/event_loop_localizer.h"
// This file tests that the full Localizer, when used with queues within the
// drivetrain, will behave properly. The purpose of this test is to make sure
@@ -20,7 +20,9 @@
namespace drivetrain {
namespace testing {
-using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+using frc971::control_loops::drivetrain::DrivetrainConfig;
+using frc971::control_loops::drivetrain::Goal;
+using frc971::control_loops::drivetrain::LocalizerControl;
namespace {
DrivetrainConfig<double> GetTest2019DrivetrainConfig() {
@@ -42,25 +44,21 @@
// We must use the 2019 drivetrain config so that we don't have to deal
// with shifting:
LocalizedDrivetrainTest()
- : ::aos::testing::ControlLoopTest(GetTest2019DrivetrainConfig().dt),
+ : ::aos::testing::ControlLoopTest(
+ aos::configuration::ReadConfig("y2019/config.json"),
+ GetTest2019DrivetrainConfig().dt),
test_event_loop_(MakeEventLoop()),
drivetrain_goal_sender_(
- test_event_loop_
- ->MakeSender<::frc971::control_loops::DrivetrainQueue::Goal>(
- ".frc971.control_loops.drivetrain_queue.goal")),
+ test_event_loop_->MakeSender<Goal>("/drivetrain")),
drivetrain_goal_fetcher_(
- test_event_loop_
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Goal>(
- ".frc971.control_loops.drivetrain_queue.goal")),
+ test_event_loop_->MakeFetcher<Goal>("/drivetrain")),
localizer_control_sender_(
- test_event_loop_->MakeSender<
- ::frc971::control_loops::drivetrain::LocalizerControl>(
- ".frc971.control_loops.drivetrain.localizer_control")),
+ test_event_loop_->MakeSender<LocalizerControl>("/drivetrain")),
drivetrain_event_loop_(MakeEventLoop()),
dt_config_(GetTest2019DrivetrainConfig()),
- camera_sender_(test_event_loop_->MakeSender<CameraFrame>(
- ".y2019.control_loops.drivetrain.camera_frames")),
+ camera_sender_(
+ test_event_loop_->MakeSender<CameraFrame>("/drivetrain")),
localizer_(drivetrain_event_loop_.get(), dt_config_),
drivetrain_(dt_config_, drivetrain_event_loop_.get(), &localizer_),
@@ -76,8 +74,8 @@
set_battery_voltage(12.0);
test_event_loop_->MakeWatcher(
- ".frc971.control_loops.drivetrain_queue.status",
- [this](const ::frc971::control_loops::DrivetrainQueue::Status &) {
+ "/drivetrain",
+ [this](const ::frc971::control_loops::drivetrain::Status &) {
// Needs to do camera updates right after we run the control loop.
if (enable_cameras_) {
SendDelayedFrames();
@@ -104,9 +102,9 @@
void VerifyNearGoal() {
drivetrain_goal_fetcher_.Fetch();
- EXPECT_NEAR(drivetrain_goal_fetcher_->left_goal,
+ EXPECT_NEAR(drivetrain_goal_fetcher_->left_goal(),
drivetrain_plant_.GetLeftPosition(), 1e-3);
- EXPECT_NEAR(drivetrain_goal_fetcher_->right_goal,
+ EXPECT_NEAR(drivetrain_goal_fetcher_->right_goal(),
drivetrain_plant_.GetRightPosition(), 1e-3);
}
@@ -125,32 +123,36 @@
robot_pose_.set_theta(drivetrain_plant_.state()(2, 0));
for (size_t ii = 0; ii < cameras_.size(); ++ii) {
const auto target_views = cameras_[ii].target_views();
- CameraFrame frame;
- frame.timestamp = chrono::duration_cast<chrono::nanoseconds>(
- monotonic_now().time_since_epoch())
- .count();
- frame.camera = ii;
- frame.num_targets = 0;
+ std::unique_ptr<CameraFrameT> frame(new CameraFrameT());
+
for (size_t jj = 0;
jj < ::std::min(EventLoopLocalizer::kMaxTargetsPerFrame,
target_views.size());
++jj) {
EventLoopLocalizer::TargetView view = target_views[jj];
- ++frame.num_targets;
+ std::unique_ptr<CameraTargetT> camera_target(new CameraTargetT());
+
const float nan = ::std::numeric_limits<float>::quiet_NaN();
if (send_bad_frames_) {
- frame.targets[jj].heading = nan;
- frame.targets[jj].distance = nan;
- frame.targets[jj].skew = nan;
- frame.targets[jj].height = nan;
+ camera_target->heading = nan;
+ camera_target->distance = nan;
+ camera_target->skew = nan;
+ camera_target->height = nan;
} else {
- frame.targets[jj].heading = view.reading.heading;
- frame.targets[jj].distance = view.reading.distance;
- frame.targets[jj].skew = view.reading.skew;
- frame.targets[jj].height = view.reading.height;
+ camera_target->heading = view.reading.heading;
+ camera_target->distance = view.reading.distance;
+ camera_target->skew = view.reading.skew;
+ camera_target->height = view.reading.height;
}
+ frame->targets.emplace_back(std::move(camera_target));
}
- camera_delay_queue_.emplace(monotonic_now(), frame);
+
+ frame->timestamp = chrono::duration_cast<chrono::nanoseconds>(
+ monotonic_now().time_since_epoch())
+ .count();
+ frame->camera = ii;
+
+ camera_delay_queue_.emplace(monotonic_now(), std::move(frame));
}
}
@@ -159,20 +161,17 @@
while (!camera_delay_queue_.empty() &&
::std::get<0>(camera_delay_queue_.front()) <
monotonic_now() - camera_latency) {
- auto message = camera_sender_.MakeMessage();
- *message = ::std::get<1>(camera_delay_queue_.front());
- ASSERT_TRUE(message.Send());
+ auto builder = camera_sender_.MakeBuilder();
+ ASSERT_TRUE(builder.Send(CameraFrame::Pack(
+ *builder.fbb(), ::std::get<1>(camera_delay_queue_.front()).get())));
camera_delay_queue_.pop();
}
}
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
- ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Goal>
- drivetrain_goal_sender_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Goal>
- drivetrain_goal_fetcher_;
- ::aos::Sender<::frc971::control_loops::drivetrain::LocalizerControl>
- localizer_control_sender_;
+ ::aos::Sender<Goal> drivetrain_goal_sender_;
+ ::aos::Fetcher<Goal> drivetrain_goal_fetcher_;
+ ::aos::Sender<LocalizerControl> localizer_control_sender_;
::std::unique_ptr<::aos::EventLoop> drivetrain_event_loop_;
const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
@@ -192,7 +191,8 @@
// A queue of camera frames so that we can add a time delay to the data
// coming from the cameras.
- ::std::queue<::std::tuple<::aos::monotonic_clock::time_point, CameraFrame>>
+ ::std::queue<::std::tuple<::aos::monotonic_clock::time_point,
+ std::unique_ptr<CameraFrameT>>>
camera_delay_queue_;
void set_enable_cameras(bool enable) { enable_cameras_ = enable; }
@@ -210,11 +210,15 @@
set_enable_cameras(false);
VerifyEstimatorAccurate(1e-10);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 1;
- message->left_goal = -1.0;
- message->right_goal = 1.0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ Goal::Builder drivetrain_builder = builder.MakeBuilder<Goal>();
+ drivetrain_builder.add_controller_type(
+ frc971::control_loops::drivetrain::ControllerType_MOTION_PROFILE);
+ drivetrain_builder.add_left_goal(-1.0);
+ drivetrain_builder.add_right_goal(1.0);
+
+ EXPECT_TRUE(builder.Send(drivetrain_builder.Finish()));
}
RunFor(chrono::seconds(3));
VerifyNearGoal();
@@ -227,11 +231,15 @@
set_enable_cameras(true);
set_bad_frames(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 1;
- message->left_goal = -1.0;
- message->right_goal = 1.0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ Goal::Builder drivetrain_builder = builder.MakeBuilder<Goal>();
+ drivetrain_builder.add_controller_type(
+ frc971::control_loops::drivetrain::ControllerType_MOTION_PROFILE);
+ drivetrain_builder.add_left_goal(-1.0);
+ drivetrain_builder.add_right_goal(1.0);
+
+ EXPECT_TRUE(builder.Send(drivetrain_builder.Finish()));
}
RunFor(chrono::seconds(3));
VerifyNearGoal();
@@ -243,11 +251,15 @@
SetEnabled(true);
set_enable_cameras(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 1;
- message->left_goal = -1.0;
- message->right_goal = 1.0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ Goal::Builder drivetrain_builder = builder.MakeBuilder<Goal>();
+ drivetrain_builder.add_controller_type(
+ frc971::control_loops::drivetrain::ControllerType_MOTION_PROFILE);
+ drivetrain_builder.add_left_goal(-1.0);
+ drivetrain_builder.add_right_goal(1.0);
+
+ EXPECT_TRUE(builder.Send(drivetrain_builder.Finish()));
}
RunFor(chrono::seconds(3));
VerifyNearGoal();
@@ -261,17 +273,20 @@
set_enable_cameras(false);
(*drivetrain_plant_.mutable_state())(0, 0) += 0.05;
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 1;
- message->left_goal = -1.0;
- message->right_goal = 1.0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ Goal::Builder drivetrain_builder = builder.MakeBuilder<Goal>();
+ drivetrain_builder.add_controller_type(
+ frc971::control_loops::drivetrain::ControllerType_MOTION_PROFILE);
+ drivetrain_builder.add_left_goal(-1.0);
+ drivetrain_builder.add_right_goal(1.0);
+
+ EXPECT_TRUE(builder.Send(drivetrain_builder.Finish()));
}
RunFor(chrono::seconds(3));
// VerifyNearGoal succeeds because it is just checking wheel positions:
VerifyNearGoal();
- const Eigen::Matrix<double, 5, 1> true_state =
- drivetrain_plant_.state();
+ const Eigen::Matrix<double, 5, 1> true_state = drivetrain_plant_.state();
// Everything but X-value should be correct:
EXPECT_NEAR(true_state.x(), localizer_.x() + 0.05, 1e-5);
EXPECT_NEAR(true_state.y(), localizer_.y(), 1e-5);
@@ -287,19 +302,28 @@
set_enable_cameras(false);
(*drivetrain_plant_.mutable_state())(0, 0) += 0.05;
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 1;
- message->left_goal = -1.0;
- message->right_goal = 1.0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ Goal::Builder drivetrain_builder = builder.MakeBuilder<Goal>();
+ drivetrain_builder.add_controller_type(
+ frc971::control_loops::drivetrain::ControllerType_MOTION_PROFILE);
+ drivetrain_builder.add_left_goal(-1.0);
+ drivetrain_builder.add_right_goal(1.0);
+
+ EXPECT_TRUE(builder.Send(drivetrain_builder.Finish()));
}
{
- auto message = localizer_control_sender_.MakeMessage();
- message->x = drivetrain_plant_.state().x();
- message->y = drivetrain_plant_.state().y();
- message->theta = drivetrain_plant_.state()(2, 0);
- ASSERT_TRUE(message.Send());
+ auto builder = localizer_control_sender_.MakeBuilder();
+
+ LocalizerControl::Builder localizer_control_builder =
+ builder.MakeBuilder<LocalizerControl>();
+
+ localizer_control_builder.add_x(drivetrain_plant_.state().x());
+ localizer_control_builder.add_y(drivetrain_plant_.state().y());
+ localizer_control_builder.add_theta(drivetrain_plant_.state()(2, 0));
+
+ EXPECT_TRUE(builder.Send(localizer_control_builder.Finish()));
}
RunFor(chrono::seconds(3));
VerifyNearGoal();
@@ -314,11 +338,15 @@
SetStartingPosition({4.0, 0.5, 0.0});
(*drivetrain_plant_.mutable_state())(0, 0) += 0.05;
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 1;
- message->left_goal = -1.0;
- message->right_goal = 1.0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ Goal::Builder drivetrain_builder = builder.MakeBuilder<Goal>();
+ drivetrain_builder.add_controller_type(
+ frc971::control_loops::drivetrain::ControllerType_MOTION_PROFILE);
+ drivetrain_builder.add_left_goal(-1.0);
+ drivetrain_builder.add_right_goal(1.0);
+
+ EXPECT_TRUE(builder.Send(drivetrain_builder.Finish()));
}
RunFor(chrono::seconds(5));
VerifyNearGoal();
@@ -326,7 +354,9 @@
}
namespace {
-EventLoopLocalizer::Pose HPSlotLeft() { return constants::Field().targets()[7].pose(); }
+EventLoopLocalizer::Pose HPSlotLeft() {
+ return constants::Field().targets()[7].pose();
+}
} // namespace
// Tests that using the line following drivetrain and just driving straight
@@ -336,10 +366,14 @@
set_enable_cameras(false);
SetStartingPosition({4, HPSlotLeft().abs_pos().y(), M_PI});
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 3;
- message->throttle = 0.5;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ Goal::Builder drivetrain_builder = builder.MakeBuilder<Goal>();
+ drivetrain_builder.add_controller_type(
+ frc971::control_loops::drivetrain::ControllerType_LINE_FOLLOWER);
+ drivetrain_builder.add_throttle(0.5);
+
+ EXPECT_TRUE(builder.Send(drivetrain_builder.Finish()));
}
RunFor(chrono::seconds(6));
diff --git a/y2019/control_loops/drivetrain/localizer_test.cc b/y2019/control_loops/drivetrain/localizer_test.cc
index 1950468..d9a55f6 100644
--- a/y2019/control_loops/drivetrain/localizer_test.cc
+++ b/y2019/control_loops/drivetrain/localizer_test.cc
@@ -141,26 +141,75 @@
}
void SetUp() {
- ::frc971::control_loops::DrivetrainQueue::Goal goal;
- goal.controller_type = 2;
- goal.spline.spline_idx = 1;
- goal.spline.spline_count = 1;
- goal.spline_handle = 1;
- ::std::copy(GetParam().control_pts_x.begin(),
- GetParam().control_pts_x.end(), goal.spline.spline_x.begin());
- ::std::copy(GetParam().control_pts_y.begin(),
- GetParam().control_pts_y.end(), goal.spline.spline_y.begin());
- spline_drivetrain_.SetGoal(goal);
+ flatbuffers::DetachedBuffer goal_buffer;
+ {
+ flatbuffers::FlatBufferBuilder fbb;
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ fbb.CreateVector<float>(GetParam().control_pts_x.begin(),
+ GetParam().control_pts_x.size());
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ fbb.CreateVector<float>(GetParam().control_pts_y.begin(),
+ GetParam().control_pts_y.size());
+
+ frc971::MultiSpline::Builder multispline_builder(fbb);
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<frc971::MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ frc971::control_loops::drivetrain::SplineGoal::Builder spline_builder(
+ fbb);
+
+ spline_builder.add_spline_idx(1);
+ spline_builder.add_spline(multispline_offset);
+
+ flatbuffers::Offset<frc971::control_loops::drivetrain::SplineGoal>
+ spline_offset = spline_builder.Finish();
+
+ frc971::control_loops::drivetrain::Goal::Builder goal_builder(fbb);
+
+ goal_builder.add_spline(spline_offset);
+ goal_builder.add_controller_type(
+ frc971::control_loops::drivetrain::ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(1);
+
+ fbb.Finish(goal_builder.Finish());
+
+ goal_buffer = fbb.Release();
+ }
+ aos::FlatbufferDetachedBuffer<frc971::control_loops::drivetrain::Goal> goal(
+ std::move(goal_buffer));
+
+ spline_drivetrain_.SetGoal(&goal.message());
// Let the spline drivetrain compute the spline.
- ::frc971::control_loops::DrivetrainQueue::Status status;
- do {
+ while (true) {
::std::this_thread::sleep_for(::std::chrono::milliseconds(5));
- spline_drivetrain_.PopulateStatus(&status);
- } while (status.trajectory_logging.planning_state !=
- (int8_t)::frc971::control_loops::drivetrain::SplineDrivetrain::
- PlanState::kPlannedTrajectory);
- spline_drivetrain_.SetGoal(goal);
+
+ flatbuffers::FlatBufferBuilder fbb;
+
+ flatbuffers::Offset<frc971::control_loops::drivetrain::TrajectoryLogging>
+ trajectory_logging_offset =
+ spline_drivetrain_.MakeTrajectoryLogging(&fbb);
+
+ ::frc971::control_loops::drivetrain::Status::Builder status_builder(fbb);
+ status_builder.add_trajectory_logging(trajectory_logging_offset);
+ spline_drivetrain_.PopulateStatus(&status_builder);
+ fbb.Finish(status_builder.Finish());
+ aos::FlatbufferDetachedBuffer<::frc971::control_loops::drivetrain::Status>
+ status(fbb.Release());
+
+ if (status.message().trajectory_logging()->planning_state() ==
+ ::frc971::control_loops::drivetrain::PlanningState_PLANNED) {
+ break;
+ }
+ }
+ spline_drivetrain_.SetGoal(&goal.message());
}
void TearDown() {
@@ -373,7 +422,7 @@
spline_drivetrain_.Update(true, known_state);
- ::frc971::control_loops::DrivetrainQueue::Output output;
+ ::frc971::control_loops::drivetrain::OutputT output;
output.left_voltage = 0;
output.right_voltage = 0;
spline_drivetrain_.SetOutput(&output);
diff --git a/y2019/control_loops/drivetrain/target_selector.cc b/y2019/control_loops/drivetrain/target_selector.cc
index 3e43db9..7f2259d 100644
--- a/y2019/control_loops/drivetrain/target_selector.cc
+++ b/y2019/control_loops/drivetrain/target_selector.cc
@@ -1,5 +1,7 @@
#include "y2019/control_loops/drivetrain/target_selector.h"
+#include "aos/json_to_flatbuffer.h"
+
namespace y2019 {
namespace control_loops {
@@ -11,10 +13,9 @@
back_viewer_({&robot_pose_, {0.0, 0.0, 0.0}, M_PI}, kFakeFov, fake_noise_,
constants::Field().targets(), {}),
hint_fetcher_(event_loop->MakeFetcher<drivetrain::TargetSelectorHint>(
- ".y2019.control_loops.drivetrain.target_selector_hint")),
- superstructure_goal_fetcher_(event_loop->MakeFetcher<
- superstructure::SuperstructureQueue::Goal>(
- ".y2019.control_loops.superstructure.superstructure_queue.goal")) {}
+ "/drivetrain")),
+ superstructure_goal_fetcher_(
+ event_loop->MakeFetcher<superstructure::Goal>("/superstructure")) {}
bool TargetSelector::UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &state,
double command_speed) {
@@ -22,14 +23,14 @@
return false;
}
if (superstructure_goal_fetcher_.Fetch()) {
- ball_mode_ = superstructure_goal_fetcher_->suction.gamepiece_mode == 0;
+ ball_mode_ = superstructure_goal_fetcher_->suction()->gamepiece_mode() == 0;
}
if (hint_fetcher_.Fetch()) {
- AOS_LOG_STRUCT(DEBUG, "selector_hint", *hint_fetcher_);
+ VLOG(1) << "selector_hint: " << aos::FlatbufferToJson(hint_fetcher_.get());
// suggested_target is unsigned so we don't check for >= 0.
- if (hint_fetcher_->suggested_target < 4) {
+ if (hint_fetcher_->suggested_target() < 4) {
target_hint_ =
- static_cast<SelectionHint>(hint_fetcher_->suggested_target);
+ static_cast<SelectionHint>(hint_fetcher_->suggested_target());
} else {
AOS_LOG(ERROR, "Got invalid suggested target.\n");
}
diff --git a/y2019/control_loops/drivetrain/target_selector.fbs b/y2019/control_loops/drivetrain/target_selector.fbs
new file mode 100644
index 0000000..d264992
--- /dev/null
+++ b/y2019/control_loops/drivetrain/target_selector.fbs
@@ -0,0 +1,23 @@
+namespace y2019.control_loops.drivetrain;
+
+// These should match the SelectionHint enum in target_selector.h.
+enum SelectionHint : byte {
+ // No selection, we should just default to whatever.
+ NONE,
+ // Near, middle, and far targets.
+ NEAR_SHIP,
+ MID_SHIP,
+ FAR_SHIP,
+ // Far side of the rocket ship.
+ FAR_ROCKET,
+}
+
+
+// A message to provide information to the target selector about what it should
+// The drivetrain listens on ".y2019.control_loops.drivetrain.target_selector_hint"
+table TargetSelectorHint {
+ // Which target we should go for:
+ suggested_target:SelectionHint;
+}
+
+root_type TargetSelectorHint;
diff --git a/y2019/control_loops/drivetrain/target_selector.h b/y2019/control_loops/drivetrain/target_selector.h
index 7f86f89..e75e122 100644
--- a/y2019/control_loops/drivetrain/target_selector.h
+++ b/y2019/control_loops/drivetrain/target_selector.h
@@ -5,8 +5,8 @@
#include "frc971/control_loops/drivetrain/localizer.h"
#include "y2019/constants.h"
#include "y2019/control_loops/drivetrain/camera.h"
-#include "y2019/control_loops/drivetrain/target_selector.q.h"
-#include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "y2019/control_loops/drivetrain/target_selector_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
namespace y2019 {
namespace control_loops {
@@ -68,8 +68,7 @@
FakeCamera back_viewer_;
::aos::Fetcher<drivetrain::TargetSelectorHint> hint_fetcher_;
- ::aos::Fetcher<superstructure::SuperstructureQueue::Goal>
- superstructure_goal_fetcher_;
+ ::aos::Fetcher<superstructure::Goal> superstructure_goal_fetcher_;
// Whether we are currently in ball mode.
bool ball_mode_ = false;
diff --git a/y2019/control_loops/drivetrain/target_selector.q b/y2019/control_loops/drivetrain/target_selector.q
deleted file mode 100644
index 021bd3a..0000000
--- a/y2019/control_loops/drivetrain/target_selector.q
+++ /dev/null
@@ -1,12 +0,0 @@
-package y2019.control_loops.drivetrain;
-
-// A message to provide information to the target selector about what it should
-// The drivetrain listens on ".y2019.control_loops.drivetrain.target_selector_hint"
-message TargetSelectorHint {
- // Which target we should go for:
- // 0 implies no selection, we should just default to whatever.
- // 1, 2, and 3 imply the near, middle, and far targets.
- // 4 implies far side of the rocket ship.
- // These should match the SelectionHint enum in target_selector.h.
- uint8_t suggested_target;
-};
diff --git a/y2019/control_loops/drivetrain/target_selector_test.cc b/y2019/control_loops/drivetrain/target_selector_test.cc
index 32e060f..2e9028f 100644
--- a/y2019/control_loops/drivetrain/target_selector_test.cc
+++ b/y2019/control_loops/drivetrain/target_selector_test.cc
@@ -1,8 +1,8 @@
#include "y2019/control_loops/drivetrain/target_selector.h"
-#include "aos/events/simulated-event-loop.h"
+#include "aos/events/simulated_event_loop.h"
#include "gtest/gtest.h"
-#include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
namespace y2019 {
namespace control_loops {
@@ -10,7 +10,6 @@
typedef ::frc971::control_loops::TypedPose<double> Pose;
typedef ::Eigen::Matrix<double, 5, 1> State;
-using SelectionHint = TargetSelector::SelectionHint;
namespace {
// Accessors to get some useful particular targets on the field:
@@ -29,7 +28,7 @@
struct TestParams {
State state;
bool ball_mode;
- SelectionHint selection_hint;
+ drivetrain::SelectionHint selection_hint;
double command_speed;
bool expect_target;
Pose expected_pose;
@@ -38,18 +37,21 @@
class TargetSelectorParamTest : public ::testing::TestWithParam<TestParams> {
public:
TargetSelectorParamTest()
- : event_loop_(this->event_loop_factory_.MakeEventLoop()),
+ : configuration_(aos::configuration::ReadConfig("y2019/config.json")),
+ event_loop_factory_(&configuration_.message()),
+ event_loop_(this->event_loop_factory_.MakeEventLoop()),
test_event_loop_(this->event_loop_factory_.MakeEventLoop()),
target_selector_hint_sender_(
test_event_loop_->MakeSender<
::y2019::control_loops::drivetrain::TargetSelectorHint>(
- ".y2019.control_loops.drivetrain.target_selector_hint")),
- superstructure_goal_sender_(test_event_loop_->MakeSender<
- ::y2019::control_loops::superstructure::
- SuperstructureQueue::Goal>(
- ".y2019.control_loops.superstructure.superstructure_queue.goal")) {}
+ "/drivetrain")),
+ superstructure_goal_sender_(
+ test_event_loop_
+ ->MakeSender<::y2019::control_loops::superstructure::Goal>(
+ "/superstructure")) {}
private:
+ aos::FlatbufferDetachedBuffer<aos::Configuration> configuration_;
::aos::SimulatedEventLoopFactory event_loop_factory_;
protected:
@@ -58,22 +60,33 @@
::aos::Sender<::y2019::control_loops::drivetrain::TargetSelectorHint>
target_selector_hint_sender_;
- ::aos::Sender<::y2019::control_loops::superstructure::SuperstructureQueue::Goal>
+ ::aos::Sender<::y2019::control_loops::superstructure::Goal>
superstructure_goal_sender_;
-
};
TEST_P(TargetSelectorParamTest, ExpectReturn) {
TargetSelector selector(event_loop_.get());
{
- auto super_goal = superstructure_goal_sender_.MakeMessage();
- super_goal->suction.gamepiece_mode = GetParam().ball_mode ? 0 : 1;
- ASSERT_TRUE(super_goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ superstructure::SuctionGoal::Builder suction_builder =
+ builder.MakeBuilder<superstructure::SuctionGoal>();
+
+ suction_builder.add_gamepiece_mode(GetParam().ball_mode ? 0 : 1);
+
+ flatbuffers::Offset<superstructure::SuctionGoal> suction_offset =
+ suction_builder.Finish();
+
+ superstructure::Goal::Builder goal_builder =
+ builder.MakeBuilder<superstructure::Goal>();
+
+ goal_builder.add_suction(suction_offset);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
{
- auto hint = target_selector_hint_sender_.MakeMessage();
- hint->suggested_target = static_cast<int>(GetParam().selection_hint);
- ASSERT_TRUE(hint.Send());
+ auto builder = target_selector_hint_sender_.MakeBuilder();
+ ASSERT_TRUE(builder.Send(drivetrain::CreateTargetSelectorHint(
+ *builder.fbb(), GetParam().selection_hint)));
}
bool expect_target = GetParam().expect_target;
const State state = GetParam().state;
@@ -105,7 +118,7 @@
// targets:
TestParams{(State() << 0.0, 0.0, 0.0, 1.0, 1.0).finished(),
/*ball_mode=*/false,
- SelectionHint::kNone,
+ drivetrain::SelectionHint_NONE,
1.0,
false,
{},
@@ -114,41 +127,43 @@
// anything.
TestParams{(State() << 4.0, 2.0, M_PI, 0.05, 0.05).finished(),
/*ball_mode=*/false,
- SelectionHint::kNone,
+ drivetrain::SelectionHint_NONE,
0.05,
false,
{},
/*expected_radius=*/0.0},
TestParams{(State() << 4.0, 2.0, M_PI, -0.05, -0.05).finished(),
/*ball_mode=*/false,
- SelectionHint::kNone,
+ drivetrain::SelectionHint_NONE,
-0.05,
false,
{},
/*expected_radius=*/0.0},
TestParams{(State() << 4.0, 2.0, M_PI, 0.5, 0.5).finished(),
- /*ball_mode=*/false, SelectionHint::kNone, 1.0, true,
- HPSlotLeft(), /*expected_radius=*/0.0},
+ /*ball_mode=*/false, drivetrain::SelectionHint_NONE, 1.0,
+ true, HPSlotLeft(), /*expected_radius=*/0.0},
// Put ourselves between the rocket and cargo ship; we should see the
// hatches driving one direction and the near cargo ship port the other.
// We also command a speed opposite the current direction of motion and
// confirm that that behaves as expected.
TestParams{(State() << 6.0, 2.0, -M_PI_2, -0.5, -0.5).finished(),
- /*ball_mode=*/false, SelectionHint::kNone, 1.0, true,
- CargoNearLeft(), /*expected_radius=*/HatchRadius()},
+ /*ball_mode=*/false, drivetrain::SelectionHint_NONE, 1.0,
+ true, CargoNearLeft(), /*expected_radius=*/HatchRadius()},
TestParams{(State() << 6.0, 2.0, M_PI_2, 0.5, 0.5).finished(),
- /*ball_mode=*/false, SelectionHint::kNone, -1.0, true,
- CargoNearLeft(), /*expected_radius=*/HatchRadius()},
+ /*ball_mode=*/false, drivetrain::SelectionHint_NONE, -1.0,
+ true, CargoNearLeft(), /*expected_radius=*/HatchRadius()},
TestParams{(State() << 6.0, 2.0, -M_PI_2, 0.5, 0.5).finished(),
- /*ball_mode=*/false, SelectionHint::kNone, -1.0, true,
- RocketHatchFarLeft(), /*expected_radius=*/HatchRadius()},
+ /*ball_mode=*/false, drivetrain::SelectionHint_NONE, -1.0,
+ true, RocketHatchFarLeft(),
+ /*expected_radius=*/HatchRadius()},
TestParams{(State() << 6.0, 2.0, M_PI_2, -0.5, -0.5).finished(),
- /*ball_mode=*/false, SelectionHint::kNone, 1.0, true,
- RocketHatchFarLeft(), /*expected_radius=*/HatchRadius()},
+ /*ball_mode=*/false, drivetrain::SelectionHint_NONE, 1.0,
+ true, RocketHatchFarLeft(),
+ /*expected_radius=*/HatchRadius()},
// And we shouldn't see anything spinning in place:
TestParams{(State() << 6.0, 2.0, M_PI_2, -0.5, 0.5).finished(),
/*ball_mode=*/false,
- SelectionHint::kNone,
+ drivetrain::SelectionHint_NONE,
0.0,
false,
{},
@@ -156,7 +171,7 @@
// Drive backwards off the field--we should not see anything.
TestParams{(State() << -0.1, 0.0, 0.0, -0.5, -0.5).finished(),
/*ball_mode=*/false,
- SelectionHint::kNone,
+ drivetrain::SelectionHint_NONE,
-1.0,
false,
{},
@@ -164,21 +179,14 @@
// In ball mode, we should be able to see the portal, and get zero
// radius.
TestParams{(State() << 6.0, 2.0, M_PI_2, 0.5, 0.5).finished(),
- /*ball_mode=*/true,
- SelectionHint::kNone,
- 1.0,
- true,
- RocketPortal(),
+ /*ball_mode=*/true, drivetrain::SelectionHint_NONE, 1.0,
+ true, RocketPortal(),
/*expected_radius=*/0.0},
// Reversing direction should get cargo ship with zero radius.
TestParams{(State() << 6.0, 2.0, M_PI_2, 0.5, 0.5).finished(),
- /*ball_mode=*/true,
- SelectionHint::kNone,
- -1.0,
- true,
- CargoNearLeft(),
- /*expected_radius=*/0.0}
- ));
+ /*ball_mode=*/true, drivetrain::SelectionHint_NONE, -1.0,
+ true, CargoNearLeft(),
+ /*expected_radius=*/0.0}));
} // namespace testing
} // namespace control_loops
diff --git a/y2019/control_loops/superstructure/BUILD b/y2019/control_loops/superstructure/BUILD
index f43b448..7cbb202 100644
--- a/y2019/control_loops/superstructure/BUILD
+++ b/y2019/control_loops/superstructure/BUILD
@@ -1,16 +1,48 @@
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:profiled_subsystem_queue",
- "//frc971/control_loops:queues",
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
+ "//frc971/control_loops:profiled_subsystem_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",
+ "//frc971/control_loops:profiled_subsystem_fbs_includes",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "superstructure_position_fbs",
+ srcs = [
+ "superstructure_position.fbs",
+ ],
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
+ "//frc971/control_loops:profiled_subsystem_fbs_includes",
],
)
@@ -24,11 +56,14 @@
],
deps = [
":collision_avoidance",
- ":superstructure_queue",
+ ":superstructure_goal_fbs",
+ ":superstructure_output_fbs",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
":vacuum",
"//aos/controls:control_loop",
- "//aos/events:event-loop",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//aos/events:event_loop",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//y2019:constants",
"//y2019:status_light",
],
@@ -39,18 +74,23 @@
srcs = [
"superstructure_lib_test.cc",
],
+ data = [
+ "//y2019: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",
"//frc971/control_loops:capped_test_plant",
"//frc971/control_loops:position_sensor_sim",
"//frc971/control_loops:team_number_test_environment",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//y2019:status_light",
"//y2019/control_loops/superstructure/intake:intake_plants",
],
@@ -64,7 +104,7 @@
deps = [
":superstructure_lib",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
],
)
@@ -77,9 +117,12 @@
"collision_avoidance.h",
],
deps = [
- ":superstructure_queue",
- "//aos/controls:control_loop_queues",
+ ":superstructure_goal_fbs",
+ ":superstructure_status_fbs",
+ "//aos/controls:control_loop",
"//frc971:constants",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
],
)
@@ -92,8 +135,11 @@
"vacuum.h",
],
deps = [
- ":superstructure_queue",
+ ":superstructure_goal_fbs",
+ ":superstructure_output_fbs",
"//aos/controls:control_loop",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
],
)
@@ -104,7 +150,8 @@
],
deps = [
":collision_avoidance",
- ":superstructure_queue",
+ ":superstructure_goal_fbs",
+ ":superstructure_status_fbs",
"//aos:math",
"//aos/testing:googletest",
],
diff --git a/y2019/control_loops/superstructure/collision_avoidance.cc b/y2019/control_loops/superstructure/collision_avoidance.cc
index eb5b591..9c2bf93 100644
--- a/y2019/control_loops/superstructure/collision_avoidance.cc
+++ b/y2019/control_loops/superstructure/collision_avoidance.cc
@@ -1,7 +1,11 @@
#include "y2019/control_loops/superstructure/collision_avoidance.h"
#include <cmath>
-#include "y2019/control_loops/superstructure/superstructure.q.h"
+
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_status_generated.h"
namespace y2019 {
namespace control_loops {
@@ -30,9 +34,9 @@
clear_max_intake_goal();
}
-bool CollisionAvoidance::IsCollided(const SuperstructureQueue::Status *status) {
- return IsCollided(status->wrist.position, status->elevator.position,
- status->intake.position, status->has_piece);
+bool CollisionAvoidance::IsCollided(const Status *status) {
+ return IsCollided(status->wrist()->position(), status->elevator()->position(),
+ status->intake()->position(), status->has_piece());
}
bool CollisionAvoidance::IsCollided(const double wrist_position,
@@ -73,13 +77,12 @@
return false;
}
-void CollisionAvoidance::UpdateGoal(
- const SuperstructureQueue::Status *status,
- const SuperstructureQueue::Goal *unsafe_goal) {
- const double wrist_position = status->wrist.position;
- const double elevator_position = status->elevator.position;
- const double intake_position = status->intake.position;
- const bool has_piece = status->has_piece;
+void CollisionAvoidance::UpdateGoal(const Status *status,
+ const Goal *unsafe_goal) {
+ const double wrist_position = status->wrist()->position();
+ const double elevator_position = status->elevator()->position();
+ const double intake_position = status->intake()->position();
+ const bool has_piece = status->has_piece();
// Start with our constraints being wide open.
clear_max_wrist_goal();
@@ -155,8 +158,8 @@
}
if (unsafe_goal) {
- const double wrist_goal = unsafe_goal->wrist.unsafe_goal;
- const double intake_goal = unsafe_goal->intake.unsafe_goal;
+ const double wrist_goal = unsafe_goal->wrist()->unsafe_goal();
+ const double intake_goal = unsafe_goal->intake()->unsafe_goal();
// Compute if we need to move the intake.
const bool intake_needs_to_move = (intake_position < kIntakeMiddleAngle) ^
diff --git a/y2019/control_loops/superstructure/collision_avoidance.h b/y2019/control_loops/superstructure/collision_avoidance.h
index 7feb45d..338864e 100644
--- a/y2019/control_loops/superstructure/collision_avoidance.h
+++ b/y2019/control_loops/superstructure/collision_avoidance.h
@@ -2,9 +2,12 @@
#define Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_COLLISION_AVOIDANCE_H_
#include <cmath>
-#include "aos/controls/control_loops.q.h"
+
#include "frc971/constants.h"
-#include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_status_generated.h"
namespace y2019 {
namespace control_loops {
@@ -18,15 +21,14 @@
CollisionAvoidance();
// Reports if the superstructure is collided.
- bool IsCollided(const SuperstructureQueue::Status *status);
+ bool IsCollided(const Status *status);
bool IsCollided(double wrist_position, double elevator_position,
double intake_position, bool has_piece);
// Checks and alters goals to make sure they're safe.
// TODO(austin): Either we will have a unit delay because this has to happen
// after the controls, or we need to be more clever about restructuring.
- void UpdateGoal(const SuperstructureQueue::Status *status,
- const SuperstructureQueue::Goal *unsafe_goal);
+ void UpdateGoal(const Status *status, const Goal *unsafe_goal);
// Returns the goals to give to the respective control loops in
// superstructure.
diff --git a/y2019/control_loops/superstructure/collision_avoidance_tests.cc b/y2019/control_loops/superstructure/collision_avoidance_tests.cc
index 82bff4c..6b1f031 100644
--- a/y2019/control_loops/superstructure/collision_avoidance_tests.cc
+++ b/y2019/control_loops/superstructure/collision_avoidance_tests.cc
@@ -1,14 +1,21 @@
#include "y2019/control_loops/superstructure/collision_avoidance.h"
#include "aos/commonmath.h"
+#include "aos/flatbuffers.h"
#include "gtest/gtest.h"
-#include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_status_generated.h"
namespace y2019 {
namespace control_loops {
namespace superstructure {
namespace testing {
+using aos::FlatbufferDetachedBuffer;
+using frc971::control_loops::AbsoluteEncoderProfiledJointStatus;
+using frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus;
+using frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
+
/*
Test List:
FullClockwiseRotationFromBottomBackIntakeIn
@@ -22,48 +29,142 @@
QuarterCounterClockwiseRotationFromBottomFrontIntakeMoving
*/
+FlatbufferDetachedBuffer<Goal> MakeZeroGoal() {
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.ForceDefaults(1);
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal> wrist_offset;
+ {
+ StaticZeroingSingleDOFProfiledSubsystemGoal::Builder wrist_builder(fbb);
+
+ wrist_builder.add_unsafe_goal(0.0);
+ wrist_offset = wrist_builder.Finish();
+ }
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset;
+ {
+ StaticZeroingSingleDOFProfiledSubsystemGoal::Builder elevator_builder(fbb);
+
+ elevator_builder.add_unsafe_goal(0.0);
+ elevator_offset = elevator_builder.Finish();
+ }
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset;
+ {
+ StaticZeroingSingleDOFProfiledSubsystemGoal::Builder intake_builder(fbb);
+
+ intake_builder.add_unsafe_goal(0.0);
+ intake_offset = intake_builder.Finish();
+ }
+
+ superstructure::Goal::Builder goal_builder(fbb);
+
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_intake(intake_offset);
+
+ fbb.Finish(goal_builder.Finish());
+ return fbb.Release();
+}
+
+FlatbufferDetachedBuffer<Status> MakeZeroStatus() {
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.ForceDefaults(1);
+
+ flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus> wrist_offset;
+ {
+ PotAndAbsoluteEncoderProfiledJointStatus::Builder wrist_builder(fbb);
+
+ wrist_builder.add_position(0.0);
+ wrist_offset = wrist_builder.Finish();
+ }
+
+ flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+ elevator_offset;
+ {
+ PotAndAbsoluteEncoderProfiledJointStatus::Builder elevator_builder(fbb);
+
+ elevator_builder.add_position(0.0);
+ elevator_offset = elevator_builder.Finish();
+ }
+
+ flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus>
+ intake_offset;
+ {
+ AbsoluteEncoderProfiledJointStatus::Builder intake_builder(fbb);
+
+ intake_builder.add_position(0.0);
+ intake_offset = intake_builder.Finish();
+ }
+
+ superstructure::Status::Builder goal_builder(fbb);
+
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_has_piece(false);
+
+ fbb.Finish(goal_builder.Finish());
+ return fbb.Release();
+}
+
class CollisionAvoidanceTests : public ::testing::Test,
public ::testing::WithParamInterface<bool> {
public:
- CollisionAvoidanceTests() { status.has_piece = GetParam(); }
+ CollisionAvoidanceTests()
+ : unsafe_goal_(MakeZeroGoal()), status_(MakeZeroStatus()) {
+ status_.mutable_message()->mutate_has_piece(GetParam());
+ }
void Iterate() {
- SuperstructureQueue::Goal safe_goal;
- bool was_collided = avoidance.IsCollided(&status);
+ FlatbufferDetachedBuffer<Goal> safe_goal = MakeZeroGoal();
+ bool was_collided = avoidance.IsCollided(&status_.message());
while (true) {
- avoidance.UpdateGoal(&status, &unsafe_goal);
+ avoidance.UpdateGoal(&status_.message(), &unsafe_goal_.message());
if (!was_collided) {
- EXPECT_FALSE(avoidance.IsCollided(&status));
+ EXPECT_FALSE(avoidance.IsCollided(&status_.message()));
} else {
- was_collided = avoidance.IsCollided(&status);
+ was_collided = avoidance.IsCollided(&status_.message());
}
- safe_goal.wrist.unsafe_goal =
- ::aos::Clip(unsafe_goal.wrist.unsafe_goal, avoidance.min_wrist_goal(),
- avoidance.max_wrist_goal());
+ safe_goal.mutable_message()->mutable_wrist()->mutate_unsafe_goal(
+ ::aos::Clip(unsafe_goal_.message().wrist()->unsafe_goal(),
+ avoidance.min_wrist_goal(), avoidance.max_wrist_goal()));
- safe_goal.elevator.unsafe_goal = ::std::max(
- unsafe_goal.elevator.unsafe_goal, avoidance.min_elevator_goal());
+ safe_goal.mutable_message()->mutable_elevator()->mutate_unsafe_goal(
+ ::std::max(unsafe_goal_.message().elevator()->unsafe_goal(),
+ avoidance.min_elevator_goal()));
- safe_goal.intake.unsafe_goal =
- ::aos::Clip(unsafe_goal.intake.unsafe_goal,
- avoidance.min_intake_goal(), avoidance.max_intake_goal());
+ safe_goal.mutable_message()->mutable_intake()->mutate_unsafe_goal(
+ ::aos::Clip(unsafe_goal_.message().intake()->unsafe_goal(),
+ avoidance.min_intake_goal(),
+ avoidance.max_intake_goal()));
- LimitedMove(&status.wrist.position, safe_goal.wrist.unsafe_goal);
- LimitedMove(&status.elevator.position, safe_goal.elevator.unsafe_goal);
- LimitedMove(&status.intake.position, safe_goal.intake.unsafe_goal);
+ status_.mutable_message()->mutable_wrist()->mutate_position(
+ LimitedMove(status_.message().wrist()->position(),
+ safe_goal.message().wrist()->unsafe_goal()));
+ status_.mutable_message()->mutable_elevator()->mutate_position(
+ LimitedMove(status_.message().elevator()->position(),
+ safe_goal.message().elevator()->unsafe_goal()));
+ status_.mutable_message()->mutable_intake()->mutate_position(
+ LimitedMove(status_.message().intake()->position(),
+ safe_goal.message().intake()->unsafe_goal()));
if (IsMoving()) {
break;
}
- past_status = status;
+ past_wrist_position_ = status_.message().wrist()->position();
+ past_elevator_position_ = status_.message().elevator()->position();
+ past_intake_position_ = status_.message().intake()->position();
}
}
bool IsMoving() {
- if ((past_status.wrist.position == status.wrist.position) &&
- (past_status.elevator.position == status.elevator.position) &&
- (past_status.intake.position == status.intake.position)) {
+ if ((past_wrist_position_ == status_.message().wrist()->position()) &&
+ (past_elevator_position_ == status_.message().elevator()->position()) &&
+ (past_intake_position_ == status_.message().intake()->position())) {
return true;
} else {
return false;
@@ -71,9 +172,11 @@
}
// provide goals and status messages
- SuperstructureQueue::Goal unsafe_goal;
- SuperstructureQueue::Status status;
- SuperstructureQueue::Status past_status;
+ FlatbufferDetachedBuffer<Goal> unsafe_goal_;
+ FlatbufferDetachedBuffer<Status> status_;
+ float past_wrist_position_ = 0;
+ float past_elevator_position_ = 0;
+ float past_intake_position_ = 0;
protected:
// setup for all tests
@@ -81,20 +184,22 @@
void CheckGoals() {
// check to see if we reached the goals
- ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
- ASSERT_NEAR(unsafe_goal.elevator.unsafe_goal, status.elevator.position,
- 0.001);
- ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
+ ASSERT_NEAR(unsafe_goal_.message().wrist()->unsafe_goal(),
+ status_.message().wrist()->position(), 0.001);
+ ASSERT_NEAR(unsafe_goal_.message().elevator()->unsafe_goal(),
+ status_.message().elevator()->position(), 0.001);
+ ASSERT_NEAR(unsafe_goal_.message().intake()->unsafe_goal(),
+ status_.message().intake()->position(), 0.001);
}
private:
- void LimitedMove(float *position, double goal) {
- if (*position + kIterationMove < goal) {
- *position += kIterationMove;
- } else if (*position - kIterationMove > goal) {
- *position -= kIterationMove;
+ float LimitedMove(float position, double goal) {
+ if (position + kIterationMove < goal) {
+ return position + kIterationMove;
+ } else if (position - kIterationMove > goal) {
+ return position - kIterationMove;
} else {
- *position = goal;
+ return goal;
}
}
@@ -105,17 +210,19 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// in.
- unsafe_goal.wrist.unsafe_goal =
- avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- unsafe_goal.elevator.unsafe_goal = 0.0;
- unsafe_goal.intake.unsafe_goal =
- avoidance.kIntakeInAngle - avoidance.kEpsIntake;
+ unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal(
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist);
+ unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal(0.0);
+ unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal(
+ avoidance.kIntakeInAngle - avoidance.kEpsIntake);
// sets the status position messgaes to be have the elevator at the bottom
// with the intake in and the wrist low back
- status.wrist.position = avoidance.kWristMinAngle + avoidance.kEpsWrist;
- status.elevator.position = 0.0;
- status.intake.position = avoidance.kIntakeInAngle - avoidance.kEpsIntake;
+ status_.mutable_message()->mutable_wrist()->mutate_position(
+ avoidance.kWristMinAngle + avoidance.kEpsWrist);
+ status_.mutable_message()->mutable_elevator()->mutate_position(0.0);
+ status_.mutable_message()->mutable_intake()->mutate_position(
+ avoidance.kIntakeInAngle - avoidance.kEpsIntake);
Iterate();
@@ -128,18 +235,19 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// out.
- unsafe_goal.wrist.unsafe_goal =
- avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- unsafe_goal.elevator.unsafe_goal = 0.0;
- unsafe_goal.intake.unsafe_goal =
- avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+ unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal(
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist);
+ unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal(0.0);
+ unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal(
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
// sets the status position messgaes to be have the elevator at the half way
// with the intake in and the wrist middle front
- status.wrist.position =
- avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0);
- status.elevator.position = 0.0;
- status.intake.position = avoidance.kIntakeOutAngle;
+ status_.mutable_message()->mutable_wrist()->mutate_position(
+ avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0));
+ status_.mutable_message()->mutable_elevator()->mutate_position(0.0);
+ status_.mutable_message()->mutable_intake()->mutate_position(
+ avoidance.kIntakeOutAngle);
Iterate();
@@ -151,18 +259,19 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// in.
- status.wrist.position = avoidance.kWristMaxAngle / 2.0;
- status.elevator.position = 0.5;
- status.intake.position =
- (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+ status_.mutable_message()->mutable_wrist()->mutate_position(
+ avoidance.kWristMaxAngle / 2.0);
+ status_.mutable_message()->mutable_elevator()->mutate_position(0.5);
+ status_.mutable_message()->mutable_intake()->mutate_position(
+ (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0);
// sets the status position messgaes to be have the elevator at the half way
// with the intake in and the wrist middle front
- unsafe_goal.wrist.unsafe_goal =
- avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- unsafe_goal.elevator.unsafe_goal = 0.0;
- unsafe_goal.intake.unsafe_goal =
- avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+ unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal(
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist);
+ unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal(0.0);
+ unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal(
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
Iterate();
@@ -174,18 +283,20 @@
FullCounterClockwiseRotationFromBottomBackIntakeIn) {
// sets the status position messgaes to be have the elevator at the bottom
// with the intake in and the wrist low back
- status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- status.elevator.position = 0.0;
- status.intake.position = avoidance.kIntakeInAngle - avoidance.kEpsIntake;
+ status_.mutable_message()->mutable_wrist()->mutate_position(
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist);
+ status_.mutable_message()->mutable_elevator()->mutate_position(0.0);
+ status_.mutable_message()->mutable_intake()->mutate_position(
+ avoidance.kIntakeInAngle - avoidance.kEpsIntake);
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// in.
- unsafe_goal.wrist.unsafe_goal =
- avoidance.kWristMinAngle + avoidance.kEpsWrist;
- unsafe_goal.elevator.unsafe_goal = 0.0;
- unsafe_goal.intake.unsafe_goal =
- avoidance.kIntakeInAngle - avoidance.kEpsIntake;
+ unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal(
+ avoidance.kWristMinAngle + avoidance.kEpsWrist);
+ unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal(0.0);
+ unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal(
+ avoidance.kIntakeInAngle - avoidance.kEpsIntake);
Iterate();
@@ -198,17 +309,19 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// out.
- unsafe_goal.wrist.unsafe_goal =
- avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0);
- unsafe_goal.elevator.unsafe_goal = 0.0;
- unsafe_goal.intake.unsafe_goal =
- avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+ unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal(
+ avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0));
+ unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal(0.0);
+ unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal(
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
// sets the status position messgaes to be have the elevator at the half way
// with the intake in and the wrist middle front
- status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- status.elevator.position = 0.0;
- status.intake.position = avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+ status_.mutable_message()->mutable_wrist()->mutate_position(
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist);
+ status_.mutable_message()->mutable_elevator()->mutate_position(0.0);
+ status_.mutable_message()->mutable_intake()->mutate_position(
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
Iterate();
@@ -221,18 +334,19 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// out.
- unsafe_goal.wrist.unsafe_goal =
- avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0);
- unsafe_goal.elevator.unsafe_goal = 0.0;
- unsafe_goal.intake.unsafe_goal =
- avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+ unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal(
+ avoidance.kWristMaxAngle - (avoidance.kEpsWrist * 2.0));
+ unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal(0.0);
+ unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal(
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
// sets the status position messgaes to be have the elevator at the half way
// with the intake in and the wrist middle front
- status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- status.elevator.position = 0.5;
- status.intake.position =
- (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+ status_.mutable_message()->mutable_wrist()->mutate_position(
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist);
+ status_.mutable_message()->mutable_elevator()->mutate_position(0.5);
+ status_.mutable_message()->mutable_intake()->mutate_position(
+ (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0);
Iterate();
@@ -244,23 +358,27 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// out.
- unsafe_goal.wrist.unsafe_goal = 4.0;
- unsafe_goal.elevator.unsafe_goal = 0.0;
- unsafe_goal.intake.unsafe_goal =
- avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+ unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal(4.0);
+ unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal(0.0);
+ unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal(
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
// sets the status position messgaes to be have the elevator at the half way
// with the intake in and the wrist middle front
- status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- status.elevator.position = 0.45;
- status.intake.position = avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+ status_.mutable_message()->mutable_wrist()->mutate_position(
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist);
+ status_.mutable_message()->mutable_elevator()->mutate_position(0.45);
+ status_.mutable_message()->mutable_intake()->mutate_position(
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
Iterate();
- ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
+ ASSERT_NEAR(unsafe_goal_.message().wrist()->unsafe_goal(),
+ status_.message().wrist()->position(), 0.001);
ASSERT_NEAR((avoidance.kElevatorClearWristDownHeight + avoidance.kEps),
- status.elevator.position, 0.001);
- ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
+ status_.message().elevator()->position(), 0.001);
+ ASSERT_NEAR(unsafe_goal_.message().intake()->unsafe_goal(),
+ status_.message().intake()->position(), 0.001);
}
// Unreasonable Wrist Goal
@@ -268,89 +386,101 @@
// changes the goals to be in the position where the angle is low front and
// the elevator is all the way at the bottom with the intake attempting to be
// out.
- unsafe_goal.wrist.unsafe_goal =
- avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- unsafe_goal.elevator.unsafe_goal = 0.0;
- unsafe_goal.intake.unsafe_goal =
- (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+ unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal(
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist);
+ unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal(0.0);
+ unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal(
+ (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0);
// sets the status position messgaes to be have the elevator at the half way
// with the intake in and the wrist middle front
- status.wrist.position = avoidance.kWristMinAngle + avoidance.kEpsWrist;
- status.elevator.position = 0.45;
- status.intake.position =
- (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+ status_.mutable_message()->mutable_wrist()->mutate_position(
+ avoidance.kWristMinAngle + avoidance.kEpsWrist);
+ status_.mutable_message()->mutable_elevator()->mutate_position(0.45);
+ status_.mutable_message()->mutable_intake()->mutate_position(
+ (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0);
Iterate();
- EXPECT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
+ EXPECT_NEAR(unsafe_goal_.message().wrist()->unsafe_goal(),
+ status_.message().wrist()->position(), 0.001);
EXPECT_NEAR((avoidance.kElevatorClearIntakeHeight + avoidance.kEps),
- status.elevator.position, 0.001);
- EXPECT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
+ status_.message().elevator()->position(), 0.001);
+ EXPECT_NEAR(unsafe_goal_.message().intake()->unsafe_goal(),
+ status_.message().intake()->position(), 0.001);
}
// Fix Collision Wrist in Elevator
TEST_P(CollisionAvoidanceTests, FixElevatorCollision) {
// changes the goals
- unsafe_goal.wrist.unsafe_goal = 3.14;
- unsafe_goal.elevator.unsafe_goal = 0.0;
- unsafe_goal.intake.unsafe_goal =
- avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+ unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal(3.14);
+ unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal(0.0);
+ unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal(
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
// sets the status position messgaes
- status.wrist.position = 4.0;
- status.elevator.position = 0.0;
- status.intake.position = avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+ status_.mutable_message()->mutable_wrist()->mutate_position(4.0);
+ status_.mutable_message()->mutable_elevator()->mutate_position(0.0);
+ status_.mutable_message()->mutable_intake()->mutate_position(
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
Iterate();
- ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
+ ASSERT_NEAR(unsafe_goal_.message().wrist()->unsafe_goal(),
+ status_.message().wrist()->position(), 0.001);
ASSERT_NEAR((avoidance.kElevatorClearWristDownHeight + avoidance.kEps),
- status.elevator.position, 0.001);
- ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
+ status_.message().elevator()->position(), 0.001);
+ ASSERT_NEAR(unsafe_goal_.message().intake()->unsafe_goal(),
+ status_.message().intake()->position(), 0.001);
}
// Fix Collision Wrist in Intake
TEST_P(CollisionAvoidanceTests, FixWristCollision) {
// changes the goals
- unsafe_goal.wrist.unsafe_goal =
- avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- unsafe_goal.elevator.unsafe_goal = 0.0;
- unsafe_goal.intake.unsafe_goal =
- (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+ unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal(
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist);
+ unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal(0.0);
+ unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal(
+ (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0);
// sets the status position messgaes
- status.wrist.position = avoidance.kWristMaxAngle - avoidance.kEpsWrist;
- status.elevator.position = 0.0;
- status.intake.position =
- (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0;
+ status_.mutable_message()->mutable_wrist()->mutate_position(
+ avoidance.kWristMaxAngle - avoidance.kEpsWrist);
+ status_.mutable_message()->mutable_elevator()->mutate_position(0.0);
+ status_.mutable_message()->mutable_intake()->mutate_position(
+ (avoidance.kIntakeOutAngle + avoidance.kIntakeInAngle) / 2.0);
Iterate();
- ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
+ ASSERT_NEAR(unsafe_goal_.message().wrist()->unsafe_goal(),
+ status_.message().wrist()->position(), 0.001);
ASSERT_NEAR((avoidance.kElevatorClearIntakeHeight + avoidance.kEps),
- status.elevator.position, 0.001);
- ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
+ status_.message().elevator()->position(), 0.001);
+ ASSERT_NEAR(unsafe_goal_.message().intake()->unsafe_goal(),
+ status_.message().intake()->position(), 0.001);
}
// Fix Collision Wrist Above Elevator
TEST_P(CollisionAvoidanceTests, FixWristElevatorCollision) {
// changes the goals
- unsafe_goal.wrist.unsafe_goal = 0.0;
- unsafe_goal.elevator.unsafe_goal = 0.0;
- unsafe_goal.intake.unsafe_goal =
- avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+ unsafe_goal_.mutable_message()->mutable_wrist()->mutate_unsafe_goal ( 0.0);
+ unsafe_goal_.mutable_message()->mutable_elevator()->mutate_unsafe_goal ( 0.0);
+ unsafe_goal_.mutable_message()->mutable_intake()->mutate_unsafe_goal (
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
// sets the status position messgaes
- status.wrist.position = 0.0;
- status.elevator.position = 0.0;
- status.intake.position = avoidance.kIntakeOutAngle + avoidance.kEpsIntake;
+ status_.mutable_message()->mutable_wrist()->mutate_position ( 0.0);
+ status_.mutable_message()->mutable_elevator()->mutate_position ( 0.0);
+ status_.mutable_message()->mutable_intake()->mutate_position (
+ avoidance.kIntakeOutAngle + avoidance.kEpsIntake);
Iterate();
- ASSERT_NEAR(unsafe_goal.wrist.unsafe_goal, status.wrist.position, 0.001);
+ ASSERT_NEAR(unsafe_goal_.message().wrist()->unsafe_goal(),
+ status_.message().wrist()->position(), 0.001);
ASSERT_NEAR((avoidance.kElevatorClearHeight + avoidance.kEps),
- status.elevator.position, 0.001);
- ASSERT_NEAR(unsafe_goal.intake.unsafe_goal, status.intake.position, 0.001);
+ status_.message().elevator()->position(), 0.001);
+ ASSERT_NEAR(unsafe_goal_.message().intake()->unsafe_goal(),
+ status_.message().intake()->position(), 0.001);
}
INSTANTIATE_TEST_CASE_P(CollisionAvoidancePieceTest, CollisionAvoidanceTests,
diff --git a/y2019/control_loops/superstructure/superstructure.cc b/y2019/control_loops/superstructure/superstructure.cc
index ff9bb13..91e1704 100644
--- a/y2019/control_loops/superstructure/superstructure.cc
+++ b/y2019/control_loops/superstructure/superstructure.cc
@@ -1,34 +1,36 @@
#include "y2019/control_loops/superstructure/superstructure.h"
-#include "aos/controls/control_loops.q.h"
-#include "aos/events/event-loop.h"
-#include "frc971/control_loops/control_loops.q.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "aos/events/event_loop.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
-#include "y2019/status_light.q.h"
+#include "y2019/status_light_generated.h"
namespace y2019 {
namespace control_loops {
namespace superstructure {
+using frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus;
+using frc971::control_loops::AbsoluteEncoderProfiledJointStatus;
+
Superstructure::Superstructure(::aos::EventLoop *event_loop,
const ::std::string &name)
- : aos::controls::ControlLoop<SuperstructureQueue>(event_loop, name),
+ : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+ name),
status_light_sender_(
- event_loop->MakeSender<::y2019::StatusLight>(".y2019.status_light")),
+ event_loop->MakeSender<::y2019::StatusLight>("/superstructure")),
drivetrain_status_fetcher_(
- event_loop
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
- ".frc971.control_loops.drivetrain_queue.status")),
+ event_loop->MakeFetcher<::frc971::control_loops::drivetrain::Status>(
+ "/drivetrain")),
elevator_(constants::GetValues().elevator.subsystem_params),
wrist_(constants::GetValues().wrist.subsystem_params),
intake_(constants::GetValues().intake),
stilts_(constants::GetValues().stilts.subsystem_params) {}
-void Superstructure::RunIteration(const SuperstructureQueue::Goal *unsafe_goal,
- const SuperstructureQueue::Position *position,
- SuperstructureQueue::Output *output,
- SuperstructureQueue::Status *status) {
+void Superstructure::RunIteration(const Goal *unsafe_goal,
+ const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) {
if (WasReset()) {
AOS_LOG(ERROR, "WPILib reset, restarting\n");
elevator_.Reset();
@@ -37,49 +39,92 @@
stilts_.Reset();
}
- elevator_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->elevator) : nullptr,
- &(position->elevator),
- output != nullptr ? &(output->elevator_voltage) : nullptr,
- &(status->elevator));
+ OutputT output_struct;
- wrist_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->wrist) : nullptr,
- &(position->wrist),
- output != nullptr ? &(output->wrist_voltage) : nullptr,
- &(status->wrist));
+ flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+ elevator_status_offset = elevator_.Iterate(
+ unsafe_goal != nullptr ? unsafe_goal->elevator() : nullptr,
+ position->elevator(),
+ output != nullptr ? &(output_struct.elevator_voltage) : nullptr,
+ status->fbb());
- intake_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->intake) : nullptr,
- &(position->intake_joint),
- output != nullptr ? &(output->intake_joint_voltage) : nullptr,
- &(status->intake));
+ flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+ wrist_status_offset = wrist_.Iterate(
+ unsafe_goal != nullptr ? unsafe_goal->wrist() : nullptr,
+ position->wrist(),
+ output != nullptr ? &(output_struct.wrist_voltage) : nullptr,
+ status->fbb());
- stilts_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->stilts) : nullptr,
- &(position->stilts),
- output != nullptr ? &(output->stilts_voltage) : nullptr,
- &(status->stilts));
+ flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus> intake_status_offset =
+ intake_.Iterate(
+ unsafe_goal != nullptr ? unsafe_goal->intake() : nullptr,
+ position->intake_joint(),
+ output != nullptr ? &(output_struct.intake_joint_voltage) : nullptr,
+ status->fbb());
- vacuum_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->suction) : nullptr,
- position->suction_pressure, output, &(status->has_piece),
+ flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+ stilts_status_offset = stilts_.Iterate(
+ unsafe_goal != nullptr ? unsafe_goal->stilts() : nullptr,
+ position->stilts(),
+ output != nullptr ? &(output_struct.stilts_voltage) : nullptr,
+ status->fbb());
+
+ bool has_piece;
+ vacuum_.Iterate(unsafe_goal != nullptr ? unsafe_goal->suction() : nullptr,
+ position->suction_pressure(), &output_struct, &has_piece,
event_loop());
- status->zeroed = status->elevator.zeroed && status->wrist.zeroed &&
- status->intake.zeroed && status->stilts.zeroed;
+ bool zeroed;
+ bool estopped;
+ {
+ PotAndAbsoluteEncoderProfiledJointStatus *elevator_status =
+ GetMutableTemporaryPointer(*status->fbb(), elevator_status_offset);
+ PotAndAbsoluteEncoderProfiledJointStatus *wrist_status =
+ GetMutableTemporaryPointer(*status->fbb(), wrist_status_offset);
+ AbsoluteEncoderProfiledJointStatus *intake_status =
+ GetMutableTemporaryPointer(*status->fbb(), intake_status_offset);
+ PotAndAbsoluteEncoderProfiledJointStatus *stilts_status =
+ GetMutableTemporaryPointer(*status->fbb(), stilts_status_offset);
- status->estopped = status->elevator.estopped || status->wrist.estopped ||
- status->intake.estopped || status->stilts.estopped;
+ zeroed = elevator_status->zeroed() && wrist_status->zeroed() &&
+ intake_status->zeroed() && stilts_status->zeroed();
+
+ estopped = elevator_status->estopped() || wrist_status->estopped() ||
+ intake_status->estopped() || stilts_status->estopped();
+ }
+
+ Status::Builder status_builder = status->MakeBuilder<Status>();
+
+ status_builder.add_zeroed(zeroed);
+ status_builder.add_estopped(estopped);
+ status_builder.add_has_piece(has_piece);
+
+ status_builder.add_elevator(elevator_status_offset);
+ status_builder.add_wrist(wrist_status_offset);
+ status_builder.add_intake(intake_status_offset);
+ status_builder.add_stilts(stilts_status_offset);
+
+ flatbuffers::Offset<Status> status_offset = status_builder.Finish();
+
+ Status *status_flatbuffer =
+ GetMutableTemporaryPointer(*status->fbb(), status_offset);
if (output) {
- if (unsafe_goal && status->intake.position > kMinIntakeAngleForRollers) {
- output->intake_roller_voltage = unsafe_goal->roller_voltage;
+ if (unsafe_goal &&
+ status_flatbuffer->intake()->position() > kMinIntakeAngleForRollers) {
+ output_struct.intake_roller_voltage = unsafe_goal->roller_voltage();
} else {
- output->intake_roller_voltage = 0.0;
+ output_struct.intake_roller_voltage = 0.0;
}
+
+ output->Send(Output::Pack(*output->fbb(), &output_struct));
}
if (unsafe_goal) {
- if (!unsafe_goal->suction.grab_piece) {
+ if (!unsafe_goal->has_suction() || !unsafe_goal->suction()->grab_piece()) {
wrist_.set_controller_index(0);
elevator_.set_controller_index(0);
- } else if (unsafe_goal->suction.gamepiece_mode == 0) {
+ } else if (unsafe_goal->suction()->gamepiece_mode() == 0) {
wrist_.set_controller_index(1);
elevator_.set_controller_index(1);
} else {
@@ -90,7 +135,7 @@
// TODO(theo) move these up when Iterate() is split
// update the goals
- collision_avoidance_.UpdateGoal(status, unsafe_goal);
+ collision_avoidance_.UpdateGoal(status_flatbuffer, unsafe_goal);
elevator_.set_min_position(collision_avoidance_.min_elevator_goal());
wrist_.set_min_position(collision_avoidance_.min_wrist_goal());
@@ -102,23 +147,23 @@
if (status && unsafe_goal) {
// Light Logic
- if (status->estopped) {
+ if (status_flatbuffer->estopped()) {
// Estop is red
SendColors(1.0, 0.0, 0.0);
} else if (drivetrain_status_fetcher_.get() &&
- drivetrain_status_fetcher_->line_follow_logging.frozen) {
+ drivetrain_status_fetcher_->line_follow_logging()->frozen()) {
// Vision align is flashing white for button pressed, purple for target
// acquired.
++line_blink_count_;
if (line_blink_count_ < 20) {
- if (drivetrain_status_fetcher_->line_follow_logging.have_target) {
+ if (drivetrain_status_fetcher_->line_follow_logging()->have_target()) {
SendColors(1.0, 0.0, 1.0);
} else {
SendColors(1.0, 1.0, 1.0);
}
} else {
// And then flash with green if we have a game piece.
- if (status->has_piece) {
+ if (status_flatbuffer->has_piece()) {
SendColors(0.0, 1.0, 0.0);
} else {
SendColors(0.0, 0.0, 0.0);
@@ -130,15 +175,17 @@
}
} else {
line_blink_count_ = 0;
- if (status->has_piece) {
+ if (status_flatbuffer->has_piece()) {
// Green if we have a game piece.
SendColors(0.0, 1.0, 0.0);
- } else if (unsafe_goal->suction.gamepiece_mode == 0 &&
- !status->has_piece) {
+ } else if ((!unsafe_goal->has_suction() ||
+ unsafe_goal->suction()->gamepiece_mode() == 0) &&
+ !status_flatbuffer->has_piece()) {
// Ball mode is orange
SendColors(1.0, 0.1, 0.0);
- } else if (unsafe_goal->suction.gamepiece_mode == 1 &&
- !status->has_piece) {
+ } else if (unsafe_goal->has_suction() &&
+ unsafe_goal->suction()->gamepiece_mode() == 1 &&
+ !status_flatbuffer->has_piece()) {
// Disk mode is deep blue
SendColors(0.05, 0.1, 0.5);
} else {
@@ -146,15 +193,20 @@
}
}
}
+
+ status->Send(status_offset);
}
void Superstructure::SendColors(float red, float green, float blue) {
- auto new_status_light = status_light_sender_.MakeMessage();
- new_status_light->red = red;
- new_status_light->green = green;
- new_status_light->blue = blue;
+ auto builder = status_light_sender_.MakeBuilder();
- if (!new_status_light.Send()) {
+ StatusLight::Builder status_light_builder =
+ builder.MakeBuilder<StatusLight>();
+ status_light_builder.add_red(red);
+ status_light_builder.add_green(green);
+ status_light_builder.add_blue(blue);
+
+ if (!builder.Send(status_light_builder.Finish())) {
AOS_LOG(ERROR, "Failed to send lights.\n");
}
}
diff --git a/y2019/control_loops/superstructure/superstructure.h b/y2019/control_loops/superstructure/superstructure.h
index aab8e7d..f4a23d9 100644
--- a/y2019/control_loops/superstructure/superstructure.h
+++ b/y2019/control_loops/superstructure/superstructure.h
@@ -2,26 +2,27 @@
#define Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
#include "aos/controls/control_loop.h"
-#include "aos/events/event-loop.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "aos/events/event_loop.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "y2019/constants.h"
#include "y2019/control_loops/superstructure/collision_avoidance.h"
-#include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_status_generated.h"
#include "y2019/control_loops/superstructure/vacuum.h"
-#include "y2019/status_light.q.h"
+#include "y2019/status_light_generated.h"
namespace y2019 {
namespace control_loops {
namespace superstructure {
class Superstructure
- : public ::aos::controls::ControlLoop<SuperstructureQueue> {
+ : public ::aos::controls::ControlLoop<Goal, Position, Status, Output> {
public:
- explicit Superstructure(
- ::aos::EventLoop *event_loop,
- const ::std::string &name =
- ".y2019.control_loops.superstructure.superstructure_queue");
+ explicit Superstructure(::aos::EventLoop *event_loop,
+ const ::std::string &name = "/superstructure");
using PotAndAbsoluteEncoderSubsystem =
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
@@ -39,16 +40,15 @@
const Vacuum &vacuum() const { return vacuum_; }
protected:
- virtual void RunIteration(const SuperstructureQueue::Goal *unsafe_goal,
- const SuperstructureQueue::Position *position,
- SuperstructureQueue::Output *output,
- 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:
void SendColors(float red, float green, float blue);
::aos::Sender<::y2019::StatusLight> status_light_sender_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
PotAndAbsoluteEncoderSubsystem elevator_;
diff --git a/y2019/control_loops/superstructure/superstructure.q b/y2019/control_loops/superstructure/superstructure.q
deleted file mode 100644
index d19c2b8..0000000
--- a/y2019/control_loops/superstructure/superstructure.q
+++ /dev/null
@@ -1,115 +0,0 @@
-package y2019.control_loops.superstructure;
-
-import "aos/controls/control_loops.q";
-import "frc971/control_loops/profiled_subsystem.q";
-
-struct SuctionGoal {
- // True = apply suction
- bool grab_piece;
-
- // 0 = ball mode
- // 1 = disk mode
-
- int32_t gamepiece_mode;
-};
-
-// Published on ".y2019.control_loops.superstructure.superstructure_queue"
-queue_group SuperstructureQueue {
- implements aos.control_loops.ControlLoop;
-
- message Goal {
- // Meters, 0 = lowest position - mechanical hard stop,
- // positive = upward
- .frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal elevator;
- // 0 = linkage on the sprocket is pointing straight up,
- // positive = forward
- .frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal intake;
- // 0 = Straight up parallel to elevator
- // Positive rotates toward intake from 0
- .frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal wrist;
-
- // Distance stilts extended out of the bottom of the robot. Positive = down.
- // 0 is the height such that the bottom of the stilts is tangent to the
- // bottom of the middle wheels.
- .frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal stilts;
-
- // Positive is rollers intaking inward.
- double roller_voltage;
-
- SuctionGoal suction;
- };
-
- message Status {
- // All subsystems know their location.
- bool zeroed;
-
- // If true, we have aborted. This is the or of all subsystem estops.
- bool estopped;
-
- // Whether suction_pressure indicates cargo is held
- bool has_piece;
-
- // Status of each subsystem.
- .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus elevator;
- .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus wrist;
- .frc971.control_loops.AbsoluteEncoderProfiledJointStatus intake;
- .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus stilts;
- };
-
- message Position {
- // Input from pressure sensor in bar
- // 1 = 1 atm, 0 = full vacuum
- float suction_pressure;
-
- // Position of the elevator, 0 at lowest position, positive when up.
- .frc971.PotAndAbsolutePosition elevator;
-
- // Position of wrist, 0 when up, positive is rotating toward the front,
- // over the top.
- .frc971.PotAndAbsolutePosition wrist;
-
- // Position of the intake. 0 when rollers are retracted, positive extended.
- .frc971.AbsolutePosition intake_joint;
-
- // Position of the stilts, 0 when retracted (defualt), positive lifts robot.
- .frc971.PotAndAbsolutePosition stilts;
-
- // True if the platform detection sensors detect the platform directly
- // below the robot right behind the left and right wheels. Useful for
- // determining when the robot is all the way on the platform.
- bool platform_left_detect;
- bool platform_right_detect;
- };
-
- message Output {
- // Voltage sent to motors moving elevator up/down. Positive is up.
- double elevator_voltage;
-
- // Voltage sent to wrist motors on elevator to rotate.
- // Positive rotates over the top towards the front of the robot.
- double wrist_voltage;
-
- // Voltage sent to motors on intake joint. Positive extends rollers.
- double intake_joint_voltage;
-
- // Voltage sent to rollers on intake. Positive rolls inward.
- double intake_roller_voltage;
-
- // Voltage sent to motors to move stilts height. Positive moves robot
- // upward.
- double stilts_voltage;
-
- // True opens solenoid (applies suction)
- // Top/bottom are when wrist is toward the front of the robot
- bool intake_suction_top;
- bool intake_suction_bottom;
-
- // Voltage sent to the vacuum pump motors.
- double pump_voltage;
- };
-
- queue Goal goal;
- queue Output output;
- queue Status status;
- queue Position position;
-};
diff --git a/y2019/control_loops/superstructure/superstructure_goal.fbs b/y2019/control_loops/superstructure/superstructure_goal.fbs
new file mode 100644
index 0000000..77e7c8d
--- /dev/null
+++ b/y2019/control_loops/superstructure/superstructure_goal.fbs
@@ -0,0 +1,37 @@
+include "frc971/control_loops/profiled_subsystem.fbs";
+
+namespace y2019.control_loops.superstructure;
+
+table SuctionGoal {
+ // True = apply suction
+ grab_piece:bool;
+
+ // 0 = ball mode
+ // 1 = disk mode
+
+ gamepiece_mode:int;
+}
+
+table Goal {
+ // Meters, 0 = lowest position - mechanical hard stop,
+ // positive = upward
+ elevator:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal;
+ // 0 = linkage on the sprocket is pointing straight up,
+ // positive = forward
+ intake:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal;
+ // 0 = Straight up parallel to elevator
+ // Positive rotates toward intake from 0
+ wrist:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal;
+
+ // Distance stilts extended out of the bottom of the robot. Positive = down.
+ // 0 is the height such that the bottom of the stilts is tangent to the
+ // bottom of the middle wheels.
+ stilts:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal;
+
+ // Positive is rollers intaking inward.
+ roller_voltage:float;
+
+ suction:SuctionGoal;
+}
+
+root_type Goal;
diff --git a/y2019/control_loops/superstructure/superstructure_lib_test.cc b/y2019/control_loops/superstructure/superstructure_lib_test.cc
index bad771e..7af8ffd 100644
--- a/y2019/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2019/control_loops/superstructure/superstructure_lib_test.cc
@@ -4,7 +4,6 @@
#include <memory>
#include "aos/controls/control_loop_test.h"
-#include "aos/queue.h"
#include "frc971/control_loops/capped_test_plant.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/control_loops/team_number_test_environment.h"
@@ -27,8 +26,12 @@
namespace chrono = ::std::chrono;
using ::aos::monotonic_clock;
+using ::frc971::CreateProfileParameters;
using ::frc971::control_loops::CappedTestPlant;
+using ::frc971::control_loops::
+ CreateStaticZeroingSingleDOFProfiledSubsystemGoal;
using ::frc971::control_loops::PositionSensorSimulator;
+using ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
typedef Superstructure::PotAndAbsoluteEncoderSubsystem
PotAndAbsoluteEncoderSubsystem;
typedef Superstructure::AbsoluteEncoderSubsystem AbsoluteEncoderSubsystem;
@@ -41,15 +44,11 @@
: event_loop_(event_loop),
dt_(dt),
superstructure_position_sender_(
- event_loop_->MakeSender<SuperstructureQueue::Position>(
- ".y2019.control_loops.superstructure.superstructure_queue."
- "position")),
- superstructure_status_fetcher_(event_loop_->MakeFetcher<
- SuperstructureQueue::Status>(
- ".y2019.control_loops.superstructure.superstructure_queue.status")),
- superstructure_output_fetcher_(event_loop_->MakeFetcher<
- SuperstructureQueue::Output>(
- ".y2019.control_loops.superstructure.superstructure_queue.output")),
+ event_loop_->MakeSender<Position>("/superstructure")),
+ superstructure_status_fetcher_(
+ event_loop_->MakeFetcher<Status>("/superstructure")),
+ superstructure_output_fetcher_(
+ event_loop_->MakeFetcher<Output>("/superstructure")),
elevator_plant_(
new CappedTestPlant(::y2019::control_loops::superstructure::
elevator::MakeElevatorPlant())),
@@ -136,16 +135,38 @@
// Sends a queue message with the position of the superstructure.
void SendPositionMessage() {
- ::aos::Sender<SuperstructureQueue::Position>::Message position =
- superstructure_position_sender_.MakeMessage();
+ ::aos::Sender<Position>::Builder builder =
+ superstructure_position_sender_.MakeBuilder();
- elevator_pot_encoder_.GetSensorValues(&position->elevator);
- wrist_pot_encoder_.GetSensorValues(&position->wrist);
- intake_pot_encoder_.GetSensorValues(&position->intake_joint);
- stilts_pot_encoder_.GetSensorValues(&position->stilts);
- position->suction_pressure = simulated_pressure_;
+ frc971::PotAndAbsolutePosition::Builder elevator_builder =
+ builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> elevator_offset =
+ elevator_pot_encoder_.GetSensorValues(&elevator_builder);
- position.Send();
+ frc971::PotAndAbsolutePosition::Builder wrist_builder =
+ builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> wrist_offset =
+ wrist_pot_encoder_.GetSensorValues(&wrist_builder);
+
+ frc971::AbsolutePosition::Builder intake_builder =
+ builder.MakeBuilder<frc971::AbsolutePosition>();
+ flatbuffers::Offset<frc971::AbsolutePosition> intake_offset =
+ intake_pot_encoder_.GetSensorValues(&intake_builder);
+
+ frc971::PotAndAbsolutePosition::Builder stilts_builder =
+ builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> stilts_offset =
+ stilts_pot_encoder_.GetSensorValues(&stilts_builder);
+
+ Position::Builder position_builder = builder.MakeBuilder<Position>();
+
+ position_builder.add_elevator(elevator_offset);
+ position_builder.add_wrist(wrist_offset);
+ position_builder.add_intake_joint(intake_offset);
+ position_builder.add_stilts(stilts_offset);
+ position_builder.add_suction_pressure(simulated_pressure_);
+
+ builder.Send(position_builder.Finish());
}
double elevator_position() const { return elevator_plant_->X(0, 0); }
@@ -183,58 +204,58 @@
const double voltage_check_elevator =
(static_cast<PotAndAbsoluteEncoderSubsystem::State>(
- superstructure_status_fetcher_->elevator.state) ==
+ superstructure_status_fetcher_->elevator()->state()) ==
PotAndAbsoluteEncoderSubsystem::State::RUNNING)
? constants::GetValues().elevator.subsystem_params.operating_voltage
: constants::GetValues().elevator.subsystem_params.zeroing_voltage;
const double voltage_check_wrist =
(static_cast<PotAndAbsoluteEncoderSubsystem::State>(
- superstructure_status_fetcher_->wrist.state) ==
+ superstructure_status_fetcher_->wrist()->state()) ==
PotAndAbsoluteEncoderSubsystem::State::RUNNING)
? constants::GetValues().wrist.subsystem_params.operating_voltage
: constants::GetValues().wrist.subsystem_params.zeroing_voltage;
const double voltage_check_intake =
(static_cast<AbsoluteEncoderSubsystem::State>(
- superstructure_status_fetcher_->intake.state) ==
+ superstructure_status_fetcher_->intake()->state()) ==
AbsoluteEncoderSubsystem::State::RUNNING)
? constants::GetValues().intake.operating_voltage
: constants::GetValues().intake.zeroing_voltage;
const double voltage_check_stilts =
(static_cast<PotAndAbsoluteEncoderSubsystem::State>(
- superstructure_status_fetcher_->stilts.state) ==
+ superstructure_status_fetcher_->stilts()->state()) ==
PotAndAbsoluteEncoderSubsystem::State::RUNNING)
? constants::GetValues().stilts.subsystem_params.operating_voltage
: constants::GetValues().stilts.subsystem_params.zeroing_voltage;
- EXPECT_NEAR(superstructure_output_fetcher_->elevator_voltage, 0.0,
+ EXPECT_NEAR(superstructure_output_fetcher_->elevator_voltage(), 0.0,
voltage_check_elevator);
- EXPECT_NEAR(superstructure_output_fetcher_->wrist_voltage, 0.0,
+ EXPECT_NEAR(superstructure_output_fetcher_->wrist_voltage(), 0.0,
voltage_check_wrist);
- EXPECT_NEAR(superstructure_output_fetcher_->intake_joint_voltage, 0.0,
+ EXPECT_NEAR(superstructure_output_fetcher_->intake_joint_voltage(), 0.0,
voltage_check_intake);
- EXPECT_NEAR(superstructure_output_fetcher_->stilts_voltage, 0.0,
+ EXPECT_NEAR(superstructure_output_fetcher_->stilts_voltage(), 0.0,
voltage_check_stilts);
::Eigen::Matrix<double, 1, 1> elevator_U;
- elevator_U << superstructure_output_fetcher_->elevator_voltage +
+ elevator_U << superstructure_output_fetcher_->elevator_voltage() +
elevator_plant_->voltage_offset();
::Eigen::Matrix<double, 1, 1> wrist_U;
- wrist_U << superstructure_output_fetcher_->wrist_voltage +
+ wrist_U << superstructure_output_fetcher_->wrist_voltage() +
wrist_plant_->voltage_offset();
::Eigen::Matrix<double, 1, 1> intake_U;
- intake_U << superstructure_output_fetcher_->intake_joint_voltage +
+ intake_U << superstructure_output_fetcher_->intake_joint_voltage() +
intake_plant_->voltage_offset();
::Eigen::Matrix<double, 1, 1> stilts_U;
- stilts_U << superstructure_output_fetcher_->stilts_voltage +
+ stilts_U << superstructure_output_fetcher_->stilts_voltage() +
stilts_plant_->voltage_offset();
elevator_plant_->Update(elevator_U);
@@ -330,19 +351,19 @@
}
private:
- void CheckCollisions(const SuperstructureQueue::Status *status) {
- ASSERT_FALSE(
- collision_avoidance_.IsCollided(wrist_position(), elevator_position(),
- intake_position(), status->has_piece));
+ void CheckCollisions(const Status *status) {
+ ASSERT_FALSE(collision_avoidance_.IsCollided(
+ wrist_position(), elevator_position(), intake_position(),
+ status->has_piece()));
}
::aos::EventLoop *event_loop_;
const chrono::nanoseconds dt_;
::aos::PhasedLoopHandler *phased_loop_handle_ = nullptr;
- ::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_;
bool first_ = true;
@@ -381,24 +402,20 @@
class SuperstructureTest : public ::aos::testing::ControlLoopTest {
protected:
SuperstructureTest()
- : ::aos::testing::ControlLoopTest(chrono::microseconds(5050)),
+ : ::aos::testing::ControlLoopTest(
+ aos::configuration::ReadConfig("y2019/config.json"),
+ chrono::microseconds(5050)),
test_event_loop_(MakeEventLoop()),
- superstructure_goal_fetcher_(test_event_loop_->MakeFetcher<
- SuperstructureQueue::Goal>(
- ".y2019.control_loops.superstructure.superstructure_queue.goal")),
- superstructure_goal_sender_(test_event_loop_->MakeSender<
- SuperstructureQueue::Goal>(
- ".y2019.control_loops.superstructure.superstructure_queue.goal")),
- superstructure_status_fetcher_(test_event_loop_->MakeFetcher<
- SuperstructureQueue::Status>(
- ".y2019.control_loops.superstructure.superstructure_queue.status")),
- superstructure_output_fetcher_(test_event_loop_->MakeFetcher<
- SuperstructureQueue::Output>(
- ".y2019.control_loops.superstructure.superstructure_queue.output")),
+ superstructure_goal_fetcher_(
+ test_event_loop_->MakeFetcher<Goal>("/superstructure")),
+ superstructure_goal_sender_(
+ test_event_loop_->MakeSender<Goal>("/superstructure")),
+ superstructure_status_fetcher_(
+ test_event_loop_->MakeFetcher<Status>("/superstructure")),
+ superstructure_output_fetcher_(
+ test_event_loop_->MakeFetcher<Output>("/superstructure")),
superstructure_position_fetcher_(
- test_event_loop_->MakeFetcher<SuperstructureQueue::Position>(
- ".y2019.control_loops.superstructure.superstructure_queue."
- "position")),
+ test_event_loop_->MakeFetcher<Position>("/superstructure")),
superstructure_event_loop_(MakeEventLoop()),
superstructure_(superstructure_event_loop_.get()),
superstructure_plant_event_loop_(MakeEventLoop()),
@@ -410,13 +427,13 @@
superstructure_goal_fetcher_.Fetch();
superstructure_status_fetcher_.Fetch();
- EXPECT_NEAR(superstructure_goal_fetcher_->elevator.unsafe_goal,
- superstructure_status_fetcher_->elevator.position, 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->wrist.unsafe_goal,
+ EXPECT_NEAR(superstructure_goal_fetcher_->elevator()->unsafe_goal(),
+ superstructure_status_fetcher_->elevator()->position(), 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->wrist()->unsafe_goal(),
superstructure_plant_.wrist_position(), 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->intake.unsafe_goal,
- superstructure_status_fetcher_->intake.position, 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->stilts.unsafe_goal,
+ EXPECT_NEAR(superstructure_goal_fetcher_->intake()->unsafe_goal(),
+ superstructure_status_fetcher_->intake()->position(), 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->stilts()->unsafe_goal(),
superstructure_plant_.stilts_position(), 0.001);
}
@@ -428,17 +445,16 @@
superstructure_status_fetcher_.Fetch();
// 2 Seconds
ASSERT_LE(i, 2 * 1.0 / .00505);
- } while (!superstructure_status_fetcher_.get()->zeroed);
+ } while (!superstructure_status_fetcher_.get()->zeroed());
}
::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<SuperstructureQueue::Output> superstructure_output_fetcher_;
- ::aos::Fetcher<SuperstructureQueue::Position>
- superstructure_position_fetcher_;
+ ::aos::Fetcher<Goal> superstructure_goal_fetcher_;
+ ::aos::Sender<Goal> superstructure_goal_sender_;
+ ::aos::Fetcher<Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<Output> superstructure_output_fetcher_;
+ ::aos::Fetcher<Position> superstructure_position_fetcher_;
// Create a control loop and simulation.
::std::unique_ptr<::aos::EventLoop> superstructure_event_loop_;
@@ -459,13 +475,29 @@
WaitUntilZeroed();
{
- auto goal = superstructure_goal_sender_.MakeMessage();
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- goal->elevator.unsafe_goal = 1.4;
- goal->wrist.unsafe_goal = 1.0;
- goal->intake.unsafe_goal = 1.1;
- goal->stilts.unsafe_goal = 0.1;
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.4);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.0);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.1);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.1);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
VerifyNearGoal();
@@ -485,24 +517,36 @@
WaitUntilZeroed();
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->elevator.unsafe_goal = 1.4;
- goal->elevator.profile_params.max_velocity = 1;
- goal->elevator.profile_params.max_acceleration = 0.5;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- goal->wrist.unsafe_goal = 1.0;
- goal->wrist.profile_params.max_velocity = 1;
- goal->wrist.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.4,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.5));
- goal->intake.unsafe_goal = 1.1;
- goal->intake.profile_params.max_velocity = 1;
- goal->intake.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.0,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.5));
- goal->stilts.unsafe_goal = 0.1;
- goal->stilts.profile_params.max_velocity = 1;
- goal->stilts.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.1,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.5));
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.1,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.5));
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Give it a lot of time to get there.
@@ -520,13 +564,33 @@
// Zero it before we move.
WaitUntilZeroed();
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->elevator.unsafe_goal = constants::Values::kElevatorRange().upper;
- goal->wrist.unsafe_goal = constants::Values::kWristRange().upper;
- goal->intake.unsafe_goal = constants::Values::kIntakeRange().upper;
- goal->stilts.unsafe_goal = constants::Values::kStiltsRange().upper;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- ASSERT_TRUE(goal.Send());
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kElevatorRange().upper);
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kWristRange().upper);
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().upper);
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kStiltsRange().upper);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(8));
VerifyNearGoal();
@@ -534,24 +598,36 @@
// Try a low acceleration move with a high max velocity and verify the
// acceleration is capped like expected.
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->elevator.unsafe_goal = constants::Values::kElevatorRange().upper;
- goal->elevator.profile_params.max_velocity = 20.0;
- goal->elevator.profile_params.max_acceleration = 0.1;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- goal->wrist.unsafe_goal = constants::Values::kWristRange().upper;
- goal->wrist.profile_params.max_velocity = 20.0;
- goal->wrist.profile_params.max_acceleration = 0.1;
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kElevatorRange().upper,
+ CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
- goal->intake.unsafe_goal = constants::Values::kIntakeRange().upper;
- goal->intake.profile_params.max_velocity = 20.0;
- goal->intake.profile_params.max_acceleration = 0.1;
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kWristRange().upper,
+ CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
- goal->stilts.unsafe_goal = constants::Values::kStiltsRange().lower;
- goal->stilts.profile_params.max_velocity = 20.0;
- goal->stilts.profile_params.max_acceleration = 0.1;
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().upper,
+ CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kStiltsRange().lower,
+ CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
superstructure_plant_.set_peak_elevator_velocity(23.0);
superstructure_plant_.set_peak_elevator_acceleration(0.2);
@@ -564,36 +640,6 @@
RunFor(chrono::seconds(8));
VerifyNearGoal();
-
- // Now do a high acceleration move with a low velocity limit.
- {
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->elevator.unsafe_goal = constants::Values::kElevatorRange().lower;
- goal->elevator.profile_params.max_velocity = 0.1;
- goal->elevator.profile_params.max_acceleration = 10.0;
-
- goal->wrist.unsafe_goal = constants::Values::kWristRange().lower;
- goal->wrist.profile_params.max_velocity = 0.1;
- goal->wrist.profile_params.max_acceleration = 10.0;
-
- goal->intake.unsafe_goal = constants::Values::kIntakeRange().lower;
- goal->intake.profile_params.max_velocity = 0.1;
- goal->intake.profile_params.max_acceleration = 10.0;
-
- goal->stilts.unsafe_goal = constants::Values::kStiltsRange().lower;
- goal->stilts.profile_params.max_velocity = 0.1;
- goal->stilts.profile_params.max_acceleration = 10.0;
- }
- superstructure_plant_.set_peak_elevator_velocity(0.2);
- superstructure_plant_.set_peak_elevator_acceleration(11.0);
- superstructure_plant_.set_peak_wrist_velocity(0.2);
- superstructure_plant_.set_peak_wrist_acceleration(11.0);
- superstructure_plant_.set_peak_intake_velocity(0.2);
- superstructure_plant_.set_peak_intake_acceleration(11.0);
- superstructure_plant_.set_peak_stilts_velocity(0.2);
- superstructure_plant_.set_peak_stilts_acceleration(11.0);
-
- VerifyNearGoal();
}
// Tests if the robot zeroes properly... maybe redundant?
@@ -604,25 +650,36 @@
superstructure_plant_.InitializeStiltsPosition(0.1);
{
- auto goal = superstructure_goal_sender_.MakeMessage();
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- goal->elevator.unsafe_goal = 1.4;
- goal->elevator.profile_params.max_velocity = 1;
- goal->elevator.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.4,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.5));
- goal->wrist.unsafe_goal = 1.0;
- goal->wrist.profile_params.max_velocity = 1;
- goal->wrist.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.0,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.5));
- goal->intake.unsafe_goal = 1.1;
- goal->intake.profile_params.max_velocity = 1;
- goal->intake.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.1,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.5));
- goal->stilts.unsafe_goal = 0.1;
- goal->stilts.profile_params.max_velocity = 1;
- goal->stilts.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.1,
+ CreateProfileParameters(*builder.fbb(), 1.0, 0.5));
- ASSERT_TRUE(goal.Send());
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitUntilZeroed();
VerifyNearGoal();
@@ -660,13 +717,30 @@
WaitUntilZeroed();
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->elevator.unsafe_goal = constants::Values::kElevatorRange().lower;
- goal->wrist.unsafe_goal = -M_PI / 3.0;
- goal->intake.unsafe_goal =
- CollisionAvoidance::kIntakeInAngle - CollisionAvoidance::kEpsIntake;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kElevatorRange().lower);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), -M_PI / 3.0);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), CollisionAvoidance::kIntakeInAngle -
+ CollisionAvoidance::kEpsIntake);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.1);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Give it a lot of time to get there.
@@ -683,37 +757,71 @@
// Get the elevator and wrist out of the way and set the Intake to where
// we should be able to spin and verify that they do
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->elevator.unsafe_goal = constants::Values::kElevatorRange().upper;
- goal->wrist.unsafe_goal = 0.0;
- goal->intake.unsafe_goal = constants::Values::kIntakeRange().upper;
- goal->roller_voltage = 6.0;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kElevatorRange().upper);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.0);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().upper);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.1);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+ goal_builder.add_roller_voltage(6.0);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
superstructure_goal_fetcher_.Fetch();
superstructure_output_fetcher_.Fetch();
- EXPECT_EQ(superstructure_output_fetcher_->intake_roller_voltage,
- superstructure_goal_fetcher_->roller_voltage);
+ EXPECT_EQ(superstructure_output_fetcher_->intake_roller_voltage(),
+ superstructure_goal_fetcher_->roller_voltage());
VerifyNearGoal();
// Move the intake where we oughtn't to spin the rollers and verify they don't
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->elevator.unsafe_goal = constants::Values::kElevatorRange().upper;
- goal->wrist.unsafe_goal = 0.0;
- goal->intake.unsafe_goal = constants::Values::kIntakeRange().lower;
- goal->roller_voltage = 6.0;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kElevatorRange().upper);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.0);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kIntakeRange().lower);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.1);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+ goal_builder.add_roller_voltage(6.0);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
superstructure_goal_fetcher_.Fetch();
superstructure_output_fetcher_.Fetch();
- EXPECT_EQ(superstructure_output_fetcher_->intake_roller_voltage, 0.0);
+ EXPECT_EQ(superstructure_output_fetcher_->intake_roller_voltage(), 0.0);
VerifyNearGoal();
}
@@ -723,10 +831,33 @@
WaitUntilZeroed();
// Turn on suction
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->suction.grab_piece = true;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.4);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.0);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.1);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.1);
+
+ flatbuffers::Offset<SuctionGoal> suction_offset =
+ CreateSuctionGoal(*builder.fbb(), true);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+ goal_builder.add_suction(suction_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(Vacuum::kTimeAtHigherVoltage - chrono::milliseconds(10));
@@ -735,7 +866,7 @@
superstructure_plant_.set_simulated_pressure(0.0);
RunFor(chrono::seconds(2));
superstructure_status_fetcher_.Fetch();
- EXPECT_TRUE(superstructure_status_fetcher_->has_piece);
+ EXPECT_TRUE(superstructure_status_fetcher_->has_piece());
}
// Tests the Vacuum backs off after acquiring a gamepiece
@@ -744,23 +875,47 @@
WaitUntilZeroed();
// Turn on suction
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->suction.grab_piece = true;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.4);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.0);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.1);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.1);
+
+ flatbuffers::Offset<SuctionGoal> suction_offset =
+ CreateSuctionGoal(*builder.fbb(), true);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+ goal_builder.add_suction(suction_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Verify that at 0 pressure after short time voltage is still high
superstructure_plant_.set_simulated_pressure(0.0);
RunFor(Vacuum::kTimeAtHigherVoltage - chrono::milliseconds(10));
superstructure_output_fetcher_.Fetch();
- EXPECT_EQ(superstructure_output_fetcher_->pump_voltage, Vacuum::kPumpVoltage);
+ EXPECT_EQ(superstructure_output_fetcher_->pump_voltage(),
+ Vacuum::kPumpVoltage);
// Verify that after waiting with a piece the pump voltage goes to the
// has piece voltage
RunFor(chrono::seconds(2));
superstructure_output_fetcher_.Fetch();
- EXPECT_EQ(superstructure_output_fetcher_->pump_voltage,
+ EXPECT_EQ(superstructure_output_fetcher_->pump_voltage(),
Vacuum::kPumpHasPieceVoltage);
}
@@ -770,30 +925,77 @@
WaitUntilZeroed();
// Turn on suction
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->suction.grab_piece = true;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.4);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.0);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.1);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.1);
+
+ flatbuffers::Offset<SuctionGoal> suction_offset =
+ CreateSuctionGoal(*builder.fbb(), true);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+ goal_builder.add_suction(suction_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Get a Gamepiece
superstructure_plant_.set_simulated_pressure(0.0);
RunFor(chrono::seconds(2));
superstructure_status_fetcher_.Fetch();
- EXPECT_TRUE(superstructure_status_fetcher_->has_piece);
+ EXPECT_TRUE(superstructure_status_fetcher_->has_piece());
// Turn off suction
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->suction.grab_piece = false;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.4);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.0);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 1.1);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), 0.1);
+
+ flatbuffers::Offset<SuctionGoal> suction_offset =
+ CreateSuctionGoal(*builder.fbb(), false);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_elevator(elevator_offset);
+ goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_stilts(stilts_offset);
+ goal_builder.add_suction(suction_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
superstructure_plant_.set_simulated_pressure(1.0);
// Run for a short while and check that voltage dropped to 0
RunFor(chrono::milliseconds(10));
superstructure_output_fetcher_.Fetch();
- EXPECT_EQ(superstructure_output_fetcher_->pump_voltage, 0.0);
+ EXPECT_EQ(superstructure_output_fetcher_->pump_voltage(), 0.0);
}
// Tests that running disabled, ya know, works
diff --git a/y2019/control_loops/superstructure/superstructure_main.cc b/y2019/control_loops/superstructure/superstructure_main.cc
index 27520c1..f831774 100644
--- a/y2019/control_loops/superstructure/superstructure_main.cc
+++ b/y2019/control_loops/superstructure/superstructure_main.cc
@@ -1,12 +1,15 @@
#include "y2019/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() {
+int main(int /*argc*/, char * /*argv*/ []) {
::aos::InitNRT(true);
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::y2019::control_loops::superstructure::Superstructure superstructure(
&event_loop);
diff --git a/y2019/control_loops/superstructure/superstructure_output.fbs b/y2019/control_loops/superstructure/superstructure_output.fbs
new file mode 100644
index 0000000..b679347
--- /dev/null
+++ b/y2019/control_loops/superstructure/superstructure_output.fbs
@@ -0,0 +1,30 @@
+namespace y2019.control_loops.superstructure;
+
+table Output {
+ // Voltage sent to motors moving elevator up/down. Positive is up.
+ elevator_voltage:double;
+
+ // Voltage sent to wrist motors on elevator to rotate.
+ // Positive rotates over the top towards the front of the robot.
+ wrist_voltage:double;
+
+ // Voltage sent to motors on intake joint. Positive extends rollers.
+ intake_joint_voltage:double;
+
+ // Voltage sent to rollers on intake. Positive rolls inward.
+ intake_roller_voltage:double;
+
+ // Voltage sent to motors to move stilts height. Positive moves robot
+ // upward.
+ stilts_voltage:double;
+
+ // True opens solenoid (applies suction)
+ // Top/bottom are when wrist is toward the front of the robot
+ intake_suction_top:bool;
+ intake_suction_bottom:bool;
+
+ // Voltage sent to the vacuum pump motors.
+ pump_voltage:double;
+}
+
+root_type Output;
diff --git a/y2019/control_loops/superstructure/superstructure_position.fbs b/y2019/control_loops/superstructure/superstructure_position.fbs
new file mode 100644
index 0000000..80a851e
--- /dev/null
+++ b/y2019/control_loops/superstructure/superstructure_position.fbs
@@ -0,0 +1,30 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2019.control_loops.superstructure;
+
+table Position {
+ // Input from pressure sensor in bar
+ // 1 = 1 atm, 0 = full vacuum
+ suction_pressure:float;
+
+ // Position of the elevator, 0 at lowest position, positive when up.
+ elevator:frc971.PotAndAbsolutePosition;
+
+ // Position of wrist, 0 when up, positive is rotating toward the front,
+ // over the top.
+ wrist:frc971.PotAndAbsolutePosition;
+
+ // Position of the intake. 0 when rollers are retracted, positive extended.
+ intake_joint:frc971.AbsolutePosition;
+
+ // Position of the stilts, 0 when retracted (defualt), positive lifts robot.
+ stilts:frc971.PotAndAbsolutePosition;
+
+ // True if the platform detection sensors detect the platform directly
+ // below the robot right behind the left and right wheels. Useful for
+ // determining when the robot is all the way on the platform.
+ platform_left_detect:bool;
+ platform_right_detect:bool;
+}
+
+root_type Position;
diff --git a/y2019/control_loops/superstructure/superstructure_status.fbs b/y2019/control_loops/superstructure/superstructure_status.fbs
new file mode 100644
index 0000000..9d3f44e
--- /dev/null
+++ b/y2019/control_loops/superstructure/superstructure_status.fbs
@@ -0,0 +1,23 @@
+include "frc971/control_loops/control_loops.fbs";
+include "frc971/control_loops/profiled_subsystem.fbs";
+
+namespace y2019.control_loops.superstructure;
+
+table Status {
+ // All subsystems know their location.
+ zeroed:bool;
+
+ // If true, we have aborted. This is the or of all subsystem estops.
+ estopped:bool;
+
+ // Whether suction_pressure indicates cargo is held
+ has_piece:bool;
+
+ // Status of each subsystem.
+ elevator:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus;
+ wrist:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus;
+ intake:frc971.control_loops.AbsoluteEncoderProfiledJointStatus;
+ stilts:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus;
+}
+
+root_type Status;
diff --git a/y2019/control_loops/superstructure/vacuum.cc b/y2019/control_loops/superstructure/vacuum.cc
index 956bb79..6d4696b 100644
--- a/y2019/control_loops/superstructure/vacuum.cc
+++ b/y2019/control_loops/superstructure/vacuum.cc
@@ -2,7 +2,10 @@
#include <chrono>
-#include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_output_generated.h"
namespace y2019 {
namespace control_loops {
@@ -16,7 +19,7 @@
constexpr aos::monotonic_clock::duration Vacuum::kReleaseTime;
void Vacuum::Iterate(const SuctionGoal *unsafe_goal, float suction_pressure,
- SuperstructureQueue::Output *output, bool *has_piece,
+ OutputT *output, bool *has_piece,
aos::EventLoop *event_loop) {
auto monotonic_now = event_loop->monotonic_now();
bool low_pump_voltage = false;
@@ -46,7 +49,7 @@
low_pump_voltage = *has_piece && filtered_pressure_ < kVacuumOnThreshold;
if (unsafe_goal && output) {
- const bool release = !unsafe_goal->grab_piece;
+ const bool release = !unsafe_goal->grab_piece();
if (release) {
last_release_time_ = monotonic_now;
@@ -58,10 +61,10 @@
output->pump_voltage =
release ? 0 : (low_pump_voltage ? kPumpHasPieceVoltage : kPumpVoltage);
- if (unsafe_goal->grab_piece && unsafe_goal->gamepiece_mode == 0) {
+ if (unsafe_goal->grab_piece() && unsafe_goal->gamepiece_mode() == 0) {
output->intake_suction_top = false;
output->intake_suction_bottom = true;
- } else if (unsafe_goal->grab_piece && unsafe_goal->gamepiece_mode == 1) {
+ } else if (unsafe_goal->grab_piece() && unsafe_goal->gamepiece_mode() == 1) {
output->intake_suction_top = true;
output->intake_suction_bottom = true;
} else {
diff --git a/y2019/control_loops/superstructure/vacuum.h b/y2019/control_loops/superstructure/vacuum.h
index e793530..c4671ab 100644
--- a/y2019/control_loops/superstructure/vacuum.h
+++ b/y2019/control_loops/superstructure/vacuum.h
@@ -1,8 +1,11 @@
#ifndef Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_VACUUM_H_
#define Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_VACUUM_H_
-#include "aos/events/event-loop.h"
-#include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "aos/events/event_loop.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_output_generated.h"
namespace y2019 {
namespace control_loops {
@@ -12,8 +15,7 @@
public:
Vacuum() {}
void Iterate(const SuctionGoal *unsafe_goal, float suction_pressure,
- SuperstructureQueue::Output *output, bool *has_piece,
- aos::EventLoop *event_loop);
+ OutputT *output, bool *has_piece, aos::EventLoop *event_loop);
// Voltage to the vaccum pump when we are attempting to acquire a piece
static constexpr double kPumpVoltage = 8.0;
diff --git a/y2019/jevois/BUILD b/y2019/jevois/BUILD
index 599bcba..0363ced 100644
--- a/y2019/jevois/BUILD
+++ b/y2019/jevois/BUILD
@@ -66,7 +66,7 @@
deps = [
"//aos/containers:sized_array",
"//aos/time",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -79,7 +79,7 @@
deps = [
"//aos/containers:sized_array",
"//aos/time:time_mcu",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index 9450b13..da43495 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -11,27 +11,30 @@
#include "aos/input/drivetrain_input.h"
#include "aos/input/joystick_input.h"
#include "aos/logging/logging.h"
-#include "aos/logging/logging.h"
#include "aos/network/team_number.h"
#include "aos/util/log_interval.h"
#include "aos/vision/events/udp.h"
#include "external/com_google_protobuf/src/google/protobuf/stubs/stringprintf.h"
-#include "frc971/autonomous/auto.q.h"
#include "frc971/autonomous/base_autonomous_actor.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/control_loops/drivetrain/localizer.q.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "y2019/control_loops/drivetrain/drivetrain_base.h"
-#include "y2019/control_loops/drivetrain/target_selector.q.h"
-#include "y2019/control_loops/superstructure/superstructure.q.h"
-#include "y2019/status_light.q.h"
+#include "y2019/control_loops/drivetrain/target_selector_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2019/status_light_generated.h"
#include "y2019/vision.pb.h"
-using ::aos::input::driver_station::ButtonLocation;
-using ::aos::input::driver_station::ControlBit;
-using ::aos::input::driver_station::JoystickAxis;
-using ::aos::input::driver_station::POVLocation;
-using ::aos::events::ProtoTXUdpSocket;
+using aos::events::ProtoTXUdpSocket;
+using aos::input::driver_station::ButtonLocation;
+using aos::input::driver_station::ControlBit;
+using aos::input::driver_station::JoystickAxis;
+using aos::input::driver_station::POVLocation;
+using frc971::CreateProfileParameters;
+using frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal;
+using frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
+using frc971::control_loops::drivetrain::LocalizerControl;
namespace chrono = ::std::chrono;
@@ -39,6 +42,8 @@
namespace input {
namespace joysticks {
+namespace superstructure = y2019::control_loops::superstructure;
+
using google::protobuf::StringPrintf;
struct ElevatorWristPosition {
@@ -143,36 +148,31 @@
target_selector_hint_sender_(
event_loop->MakeSender<
::y2019::control_loops::drivetrain::TargetSelectorHint>(
- ".y2019.control_loops.drivetrain.target_selector_hint")),
+ "/drivetrain")),
localizer_control_sender_(
- event_loop->MakeSender<
- ::frc971::control_loops::drivetrain::LocalizerControl>(
- ".frc971.control_loops.drivetrain.localizer_control")),
+ event_loop->MakeSender<LocalizerControl>("/drivetrain")),
camera_log_sender_(
- event_loop->MakeSender<::y2019::CameraLog>(".y2019.camera_log")),
- superstructure_goal_fetcher_(event_loop->MakeFetcher<
- ::y2019::control_loops::superstructure::
- SuperstructureQueue::Goal>(
- ".y2019.control_loops.superstructure.superstructure_queue.goal")),
- superstructure_goal_sender_(event_loop->MakeSender<
- ::y2019::control_loops::superstructure::
- SuperstructureQueue::Goal>(
- ".y2019.control_loops.superstructure.superstructure_queue.goal")),
+ event_loop->MakeSender<::y2019::CameraLog>("/camera")),
+ superstructure_goal_fetcher_(
+ event_loop->MakeFetcher<superstructure::Goal>("/superstructure")),
+ superstructure_goal_sender_(
+ event_loop->MakeSender<superstructure::Goal>("/superstructure")),
superstructure_position_fetcher_(
- event_loop->MakeFetcher<::y2019::control_loops::superstructure::
- SuperstructureQueue::Position>(
- ".y2019.control_loops.superstructure.superstructure_queue."
- "position")),
+ event_loop->MakeFetcher<superstructure::Position>(
+ "/superstructure")),
superstructure_status_fetcher_(
- event_loop->MakeFetcher<::y2019::control_loops::superstructure::
- SuperstructureQueue::Status>(
- ".y2019.control_loops.superstructure.superstructure_queue."
- "status")) {
+ event_loop->MakeFetcher<superstructure::Status>(
+ "/superstructure")) {
const uint16_t team = ::aos::network::GetTeamNumber();
superstructure_goal_fetcher_.Fetch();
if (superstructure_goal_fetcher_.get()) {
- grab_piece_ = superstructure_goal_fetcher_->suction.grab_piece;
- switch_ball_ = superstructure_goal_fetcher_->suction.gamepiece_mode == 0;
+ grab_piece_ = superstructure_goal_fetcher_->has_suction()
+ ? superstructure_goal_fetcher_->suction()->grab_piece()
+ : false;
+ switch_ball_ =
+ superstructure_goal_fetcher_->has_suction()
+ ? (superstructure_goal_fetcher_->suction()->gamepiece_mode() == 0)
+ : true;
}
video_tx_.reset(new ProtoTXUdpSocket<VisionControl>(
StringPrintf("10.%d.%d.179", team / 100, team % 100), 5000));
@@ -195,106 +195,172 @@
return;
}
- if (!superstructure_status_fetcher_->has_piece) {
+ CHECK(superstructure_status_fetcher_->has_stilts());
+
+ if (!superstructure_status_fetcher_->has_piece()) {
last_not_has_piece_ = monotonic_now;
}
- auto new_superstructure_goal = superstructure_goal_sender_.MakeMessage();
+ auto main_superstructure_goal_builder =
+ superstructure_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<superstructure::Goal> superstructure_goal_offset;
{
- auto target_hint = target_selector_hint_sender_.MakeMessage();
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ elevator_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *main_superstructure_goal_builder.fbb(), 0.0,
+ CreateProfileParameters(*main_superstructure_goal_builder.fbb(),
+ 0.0, 0.0));
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *main_superstructure_goal_builder.fbb(), -1.2);
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *main_superstructure_goal_builder.fbb(), 0.0,
+ CreateProfileParameters(*main_superstructure_goal_builder.fbb(),
+ 0.0, 0.0));
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ stilts_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *main_superstructure_goal_builder.fbb(), 0.0,
+ CreateProfileParameters(*main_superstructure_goal_builder.fbb(),
+ 0.0, 0.0));
+
+ superstructure::Goal::Builder superstructure_goal_builder =
+ main_superstructure_goal_builder.MakeBuilder<superstructure::Goal>();
+
+ superstructure_goal_builder.add_elevator(elevator_offset);
+ superstructure_goal_builder.add_intake(intake_offset);
+ superstructure_goal_builder.add_wrist(wrist_offset);
+ superstructure_goal_builder.add_stilts(stilts_offset);
+ superstructure_goal_builder.add_roller_voltage(0.0);
+
+ superstructure_goal_offset = superstructure_goal_builder.Finish();
+ }
+ superstructure::Goal *mutable_superstructure_goal =
+ GetMutableTemporaryPointer(*main_superstructure_goal_builder.fbb(),
+ superstructure_goal_offset);
+
+ {
+ auto builder = target_selector_hint_sender_.MakeBuilder();
+ control_loops::drivetrain::TargetSelectorHint::Builder
+ target_selector_hint_builder =
+ builder
+ .MakeBuilder<control_loops::drivetrain::TargetSelectorHint>();
if (data.IsPressed(kNearCargoHint)) {
- target_hint->suggested_target = 1;
+ target_selector_hint_builder.add_suggested_target(
+ control_loops::drivetrain::SelectionHint_NEAR_SHIP);
} else if (data.IsPressed(kMidCargoHint)) {
- target_hint->suggested_target = 2;
+ target_selector_hint_builder.add_suggested_target(
+ control_loops::drivetrain::SelectionHint_MID_SHIP);
} else if (data.IsPressed(kFarCargoHint)) {
- target_hint->suggested_target = 3;
+ target_selector_hint_builder.add_suggested_target(
+ control_loops::drivetrain::SelectionHint_FAR_SHIP);
} else {
const double cargo_joy_y = data.GetAxis(kCargoSelectorY);
const double cargo_joy_x = data.GetAxis(kCargoSelectorX);
if (cargo_joy_y > 0.5) {
- target_hint->suggested_target = 1;
+ target_selector_hint_builder.add_suggested_target(
+ control_loops::drivetrain::SelectionHint_NEAR_SHIP);
} else if (cargo_joy_y < -0.5) {
- target_hint->suggested_target = 3;
+ target_selector_hint_builder.add_suggested_target(
+ control_loops::drivetrain::SelectionHint_FAR_SHIP);
} else if (::std::abs(cargo_joy_x) > 0.5) {
- target_hint->suggested_target = 2;
+ target_selector_hint_builder.add_suggested_target(
+ control_loops::drivetrain::SelectionHint_MID_SHIP);
} else {
- target_hint->suggested_target = 0;
+ target_selector_hint_builder.add_suggested_target(
+ control_loops::drivetrain::SelectionHint_NONE);
}
}
- if (!target_hint.Send()) {
+ if (!builder.Send(target_selector_hint_builder.Finish())) {
AOS_LOG(ERROR, "Failed to send target selector hint.\n");
}
}
if (data.PosEdge(kResetLocalizerLeft)) {
- auto localizer_resetter = localizer_control_sender_.MakeMessage();
+ auto builder = localizer_control_sender_.MakeBuilder();
// Start at the left feeder station.
- localizer_resetter->x = 0.6;
- localizer_resetter->y = 3.4;
- localizer_resetter->keep_current_theta = true;
+ LocalizerControl::Builder localizer_control_builder =
+ builder.MakeBuilder<LocalizerControl>();
+ localizer_control_builder.add_x(0.6);
+ localizer_control_builder.add_y(3.4);
+ localizer_control_builder.add_keep_current_theta(true);
- if (!localizer_resetter.Send()) {
+ if (!builder.Send(localizer_control_builder.Finish())) {
AOS_LOG(ERROR, "Failed to reset localizer.\n");
}
}
if (data.PosEdge(kResetLocalizerRight)) {
- auto localizer_resetter = localizer_control_sender_.MakeMessage();
- // Start at the left feeder station.
- localizer_resetter->x = 0.6;
- localizer_resetter->y = -3.4;
- localizer_resetter->keep_current_theta = true;
+ auto builder = localizer_control_sender_.MakeBuilder();
+ // Start at the right feeder station.
+ LocalizerControl::Builder localizer_control_builder =
+ builder.MakeBuilder<LocalizerControl>();
+ localizer_control_builder.add_x(0.6);
+ localizer_control_builder.add_y(-3.4);
+ localizer_control_builder.add_keep_current_theta(true);
- if (!localizer_resetter.Send()) {
+ if (!builder.Send(localizer_control_builder.Finish())) {
AOS_LOG(ERROR, "Failed to reset localizer.\n");
}
}
if (data.PosEdge(kResetLocalizerLeftForwards)) {
- auto localizer_resetter = localizer_control_sender_.MakeMessage();
+ auto builder = localizer_control_sender_.MakeBuilder();
// Start at the left feeder station.
- localizer_resetter->x = 0.4;
- localizer_resetter->y = 3.4;
- localizer_resetter->theta = 0.0;
+ LocalizerControl::Builder localizer_control_builder =
+ builder.MakeBuilder<LocalizerControl>();
+ localizer_control_builder.add_x(0.4);
+ localizer_control_builder.add_y(3.4);
+ localizer_control_builder.add_theta(0.0);
- if (!localizer_resetter.Send()) {
+ if (!builder.Send(localizer_control_builder.Finish())) {
AOS_LOG(ERROR, "Failed to reset localizer.\n");
}
}
if (data.PosEdge(kResetLocalizerLeftBackwards)) {
- auto localizer_resetter = localizer_control_sender_.MakeMessage();
+ auto builder = localizer_control_sender_.MakeBuilder();
// Start at the left feeder station.
- localizer_resetter->x = 0.4;
- localizer_resetter->y = 3.4;
- localizer_resetter->theta = M_PI;
+ LocalizerControl::Builder localizer_control_builder =
+ builder.MakeBuilder<LocalizerControl>();
+ localizer_control_builder.add_x(0.4);
+ localizer_control_builder.add_y(3.4);
+ localizer_control_builder.add_theta(M_PI);
- if (!localizer_resetter.Send()) {
+ if (!builder.Send(localizer_control_builder.Finish())) {
AOS_LOG(ERROR, "Failed to reset localizer.\n");
}
}
if (data.PosEdge(kResetLocalizerRightForwards)) {
- auto localizer_resetter = localizer_control_sender_.MakeMessage();
+ auto builder = localizer_control_sender_.MakeBuilder();
// Start at the right feeder station.
- localizer_resetter->x = 0.4;
- localizer_resetter->y = -3.4;
- localizer_resetter->theta = 0.0;
+ LocalizerControl::Builder localizer_control_builder =
+ builder.MakeBuilder<LocalizerControl>();
+ localizer_control_builder.add_x(0.4);
+ localizer_control_builder.add_y(-3.4);
+ localizer_control_builder.add_theta(0.0);
- if (!localizer_resetter.Send()) {
+ if (!builder.Send(localizer_control_builder.Finish())) {
AOS_LOG(ERROR, "Failed to reset localizer.\n");
}
}
if (data.PosEdge(kResetLocalizerRightBackwards)) {
- auto localizer_resetter = localizer_control_sender_.MakeMessage();
+ auto builder = localizer_control_sender_.MakeBuilder();
// Start at the right feeder station.
- localizer_resetter->x = 0.4;
- localizer_resetter->y = -3.4;
- localizer_resetter->theta = M_PI;
+ LocalizerControl::Builder localizer_control_builder =
+ builder.MakeBuilder<LocalizerControl>();
+ localizer_control_builder.add_x(0.4);
+ localizer_control_builder.add_y(-3.4);
+ localizer_control_builder.add_theta(M_PI);
- if (!localizer_resetter.Send()) {
+ if (!builder.Send(localizer_control_builder.Finish())) {
AOS_LOG(ERROR, "Failed to reset localizer.\n");
}
}
@@ -302,7 +368,7 @@
if (data.PosEdge(kRelease) &&
monotonic_now >
last_release_button_press_ + ::std::chrono::milliseconds(500)) {
- if (superstructure_status_fetcher_->has_piece) {
+ if (superstructure_status_fetcher_->has_piece()) {
release_mode_ = ReleaseButtonMode::kRelease;
} else {
release_mode_ = ReleaseButtonMode::kBallIntake;
@@ -313,7 +379,8 @@
last_release_button_press_ = monotonic_now;
}
- AOS_LOG(INFO, "has_piece: %d\n", superstructure_status_fetcher_->has_piece);
+ AOS_LOG(INFO, "has_piece: %d\n",
+ superstructure_status_fetcher_->has_piece());
if (data.IsPressed(kSuctionBall)) {
grab_piece_ = true;
} else if (data.IsPressed(kSuctionHatch)) {
@@ -321,7 +388,7 @@
} else if ((release_mode_ == ReleaseButtonMode::kRelease &&
data.IsPressed(kRelease)) ||
data.IsPressed(kReleaseButtonBoard) ||
- !superstructure_status_fetcher_->has_piece) {
+ !superstructure_status_fetcher_->has_piece()) {
grab_piece_ = false;
AOS_LOG(INFO, "releasing due to other thing\n");
}
@@ -329,8 +396,8 @@
if (data.IsPressed(kRocketBackwardUnpressed)) {
elevator_wrist_pos_ = kStowPos;
}
- new_superstructure_goal->intake.unsafe_goal = -1.2;
- new_superstructure_goal->roller_voltage = 0.0;
+ mutable_superstructure_goal->mutable_intake()->mutate_unsafe_goal(-1.2);
+ mutable_superstructure_goal->mutate_roller_voltage(0.0);
const bool kDoBallIntake =
(!climbed_ && release_mode_ == ReleaseButtonMode::kBallIntake &&
@@ -345,8 +412,10 @@
}
if (switch_ball_) {
- if (superstructure_status_fetcher_->has_piece) {
- new_superstructure_goal->wrist.profile_params.max_acceleration = 20;
+ if (superstructure_status_fetcher_->has_piece()) {
+ mutable_superstructure_goal->mutable_wrist()
+ ->mutable_profile_params()
+ ->mutate_max_acceleration(20);
}
// Go to intake position and apply vacuum
@@ -411,18 +480,18 @@
if (kDoBallOutake ||
(kDoBallIntake &&
monotonic_now < last_not_has_piece_ + chrono::milliseconds(200))) {
- new_superstructure_goal->intake.unsafe_goal = 0.83;
+ mutable_superstructure_goal->mutable_intake()->mutate_unsafe_goal(0.83);
}
- if (kDoBallIntake && !superstructure_status_fetcher_->has_piece) {
+ if (kDoBallIntake && !superstructure_status_fetcher_->has_piece()) {
elevator_wrist_pos_ = kBallIntakePos;
- new_superstructure_goal->roller_voltage = 9.0;
+ mutable_superstructure_goal->mutate_roller_voltage(9.0);
grab_piece_ = true;
} else {
if (kDoBallOutake) {
- new_superstructure_goal->roller_voltage = -6.0;
+ mutable_superstructure_goal->mutate_roller_voltage(-6.0);
} else {
- new_superstructure_goal->roller_voltage = 0.0;
+ mutable_superstructure_goal->mutate_roller_voltage(0.0);
}
}
}
@@ -430,47 +499,70 @@
constexpr double kDeployStiltPosition = 0.5;
if (data.IsPressed(kFallOver)) {
- new_superstructure_goal->stilts.unsafe_goal = 0.71;
- new_superstructure_goal->stilts.profile_params.max_velocity = 0.65;
- new_superstructure_goal->stilts.profile_params.max_acceleration = 1.25;
+ mutable_superstructure_goal->mutable_stilts()->mutate_unsafe_goal(0.71);
+ mutable_superstructure_goal->mutable_stilts()
+ ->mutable_profile_params()
+ ->mutate_max_velocity(0.65);
+ mutable_superstructure_goal->mutable_stilts()
+ ->mutable_profile_params()
+ ->mutate_max_acceleration(1.25);
} else if (data.IsPressed(kHalfStilt)) {
was_above_ = false;
- new_superstructure_goal->stilts.unsafe_goal = 0.345;
- new_superstructure_goal->stilts.profile_params.max_velocity = 0.65;
+ mutable_superstructure_goal->mutable_stilts()->mutate_unsafe_goal ( 0.345);
+ mutable_superstructure_goal->mutable_stilts()->mutable_profile_params()->mutate_max_velocity ( 0.65);
} else if (data.IsPressed(kDeployStilt) || was_above_) {
- new_superstructure_goal->stilts.unsafe_goal = kDeployStiltPosition;
- new_superstructure_goal->stilts.profile_params.max_velocity = 0.65;
- new_superstructure_goal->stilts.profile_params.max_acceleration = 2.0;
+ mutable_superstructure_goal->mutable_stilts()->mutate_unsafe_goal(
+ kDeployStiltPosition);
+ mutable_superstructure_goal->mutable_stilts()
+ ->mutable_profile_params()
+ ->mutate_max_velocity(0.65);
+ mutable_superstructure_goal->mutable_stilts()
+ ->mutable_profile_params()
+ ->mutate_max_acceleration(2.0);
} else {
- new_superstructure_goal->stilts.unsafe_goal = 0.005;
- new_superstructure_goal->stilts.profile_params.max_velocity = 0.65;
- new_superstructure_goal->stilts.profile_params.max_acceleration = 2.0;
+ mutable_superstructure_goal->mutable_stilts()->mutate_unsafe_goal(0.005);
+ mutable_superstructure_goal->mutable_stilts()
+ ->mutable_profile_params()
+ ->mutate_max_velocity(0.65);
+ mutable_superstructure_goal->mutable_stilts()
+ ->mutable_profile_params()
+ ->mutate_max_acceleration(2.0);
}
- if (superstructure_status_fetcher_->stilts.position > 0.1) {
+ if (superstructure_status_fetcher_->stilts()->position() > 0.1) {
elevator_wrist_pos_ = kClimbPos;
climbed_ = true;
- new_superstructure_goal->wrist.profile_params.max_acceleration = 10;
- new_superstructure_goal->elevator.profile_params.max_acceleration = 6;
+ mutable_superstructure_goal->mutable_wrist()
+ ->mutable_profile_params()
+ ->mutate_max_acceleration(10);
+ mutable_superstructure_goal->mutable_elevator()
+ ->mutable_profile_params()
+ ->mutate_max_acceleration(6);
}
// If we've been asked to go above deploy and made it up that high, latch
// was_above.
- if (new_superstructure_goal->stilts.unsafe_goal > kDeployStiltPosition &&
- superstructure_status_fetcher_->stilts.position >=
+ if (mutable_superstructure_goal->stilts()->unsafe_goal() >
+ kDeployStiltPosition &&
+ superstructure_status_fetcher_->stilts()->position() >=
kDeployStiltPosition) {
was_above_ = true;
- } else if ((superstructure_position_fetcher_->platform_left_detect &&
- superstructure_position_fetcher_->platform_right_detect) &&
+ } else if ((superstructure_position_fetcher_->platform_left_detect() &&
+ superstructure_position_fetcher_->platform_right_detect()) &&
!data.IsPressed(kDeployStilt) && !data.IsPressed(kFallOver)) {
was_above_ = false;
}
- if (superstructure_status_fetcher_->stilts.position >
+ if (superstructure_status_fetcher_->stilts()->position() >
kDeployStiltPosition &&
- new_superstructure_goal->stilts.unsafe_goal == kDeployStiltPosition) {
- new_superstructure_goal->stilts.profile_params.max_velocity = 0.30;
- new_superstructure_goal->stilts.profile_params.max_acceleration = 0.75;
+ mutable_superstructure_goal->stilts()->unsafe_goal() ==
+ kDeployStiltPosition) {
+ mutable_superstructure_goal->mutable_stilts()
+ ->mutable_profile_params()
+ ->mutate_max_velocity(0.30);
+ mutable_superstructure_goal->mutable_stilts()
+ ->mutable_profile_params()
+ ->mutate_max_acceleration(0.75);
}
if ((release_mode_ == ReleaseButtonMode::kRelease &&
@@ -481,21 +573,22 @@
}
if (switch_ball_) {
- new_superstructure_goal->suction.gamepiece_mode = 0;
+ mutable_superstructure_goal->mutable_suction()->mutate_gamepiece_mode(0);
} else {
- new_superstructure_goal->suction.gamepiece_mode = 1;
+ mutable_superstructure_goal->mutable_suction()->mutate_gamepiece_mode(1);
}
vision_control_.set_flip_image(elevator_wrist_pos_.wrist < 0);
- new_superstructure_goal->suction.grab_piece = grab_piece_;
+ mutable_superstructure_goal->mutable_suction()->mutate_grab_piece(
+ grab_piece_);
- new_superstructure_goal->elevator.unsafe_goal =
- elevator_wrist_pos_.elevator;
- new_superstructure_goal->wrist.unsafe_goal = elevator_wrist_pos_.wrist;
+ mutable_superstructure_goal->mutable_elevator()->mutate_unsafe_goal(
+ elevator_wrist_pos_.elevator);
+ mutable_superstructure_goal->mutable_wrist()->mutate_unsafe_goal(
+ elevator_wrist_pos_.wrist);
- AOS_LOG_STRUCT(DEBUG, "sending goal", *new_superstructure_goal);
- if (!new_superstructure_goal.Send()) {
+ if (!main_superstructure_goal_builder.Send(superstructure_goal_offset)) {
AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
}
@@ -506,10 +599,8 @@
}
{
- auto camera_log_message = camera_log_sender_.MakeMessage();
- camera_log_message->log = data.IsPressed(kCameraLog);
- AOS_LOG_STRUCT(DEBUG, "camera_log", *camera_log_message);
- camera_log_message.Send();
+ auto builder = camera_log_sender_.MakeBuilder();
+ builder.Send(CreateCameraLog(*builder.fbb(), data.IsPressed(kCameraLog)));
}
}
@@ -517,26 +608,16 @@
::aos::Sender<::y2019::control_loops::drivetrain::TargetSelectorHint>
target_selector_hint_sender_;
- ::aos::Sender<::frc971::control_loops::drivetrain::LocalizerControl>
- localizer_control_sender_;
+ ::aos::Sender<LocalizerControl> localizer_control_sender_;
::aos::Sender<::y2019::CameraLog> camera_log_sender_;
- ::aos::Fetcher<
- ::y2019::control_loops::superstructure::SuperstructureQueue::Goal>
- superstructure_goal_fetcher_;
+ ::aos::Fetcher<superstructure::Goal> superstructure_goal_fetcher_;
- ::aos::Sender<
- ::y2019::control_loops::superstructure::SuperstructureQueue::Goal>
- superstructure_goal_sender_;
+ ::aos::Sender<superstructure::Goal> superstructure_goal_sender_;
- ::aos::Fetcher<
- ::y2019::control_loops::superstructure::SuperstructureQueue::Position>
- superstructure_position_fetcher_;
- ::aos::Fetcher<
- ::y2019::control_loops::superstructure::SuperstructureQueue::Status>
- superstructure_status_fetcher_;
-
+ ::aos::Fetcher<superstructure::Position> superstructure_position_fetcher_;
+ ::aos::Fetcher<superstructure::Status> superstructure_status_fetcher_;
// Bool to track if we've been above the deploy position. Once this bool is
// set, don't let the stilts retract until we see the platform.
@@ -576,7 +657,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());
::y2019::input::joysticks::Reader reader(&event_loop);
event_loop.Run();
diff --git a/y2019/status_light.fbs b/y2019/status_light.fbs
new file mode 100644
index 0000000..633b030
--- /dev/null
+++ b/y2019/status_light.fbs
@@ -0,0 +1,15 @@
+namespace y2019;
+
+table StatusLight {
+ // How bright to make each one. 0 is off, 1 is full on.
+ red:float;
+ green:float;
+ blue:float;
+}
+
+table CameraLog {
+ log:bool;
+}
+
+root_type CameraLog;
+root_type StatusLight;
diff --git a/y2019/status_light.q b/y2019/status_light.q
deleted file mode 100644
index 66e8ad7..0000000
--- a/y2019/status_light.q
+++ /dev/null
@@ -1,14 +0,0 @@
-package y2019;
-
-// Published on ".y2019.status_light"
-message StatusLight {
- // How bright to make each one. 0 is off, 1 is full on.
- float red;
- float green;
- float blue;
-};
-
-// Published on ".y2019.camera_log"
-message CameraLog {
- bool log;
-};
diff --git a/y2019/vision/BUILD b/y2019/vision/BUILD
index c7f0009..22b973a 100644
--- a/y2019/vision/BUILD
+++ b/y2019/vision/BUILD
@@ -1,4 +1,3 @@
-load("//aos/build:queues.bzl", "queue_library")
load("//tools/build_rules:gtk_dependent.bzl", "gtk_dependent_cc_binary", "gtk_dependent_cc_library")
load("@com_google_protobuf//:protobuf.bzl", "cc_proto_library")
load("//tools:environments.bzl", "mcu_cpus")
diff --git a/y2019/vision/global_calibration.cc b/y2019/vision/global_calibration.cc
index 304f4cb..ac559b0 100644
--- a/y2019/vision/global_calibration.cc
+++ b/y2019/vision/global_calibration.cc
@@ -11,9 +11,6 @@
#include "aos/vision/image/reader.h"
#include "y2019/vision/target_finder.h"
-#undef CHECK_NOTNULL
-#undef CHECK_OP
-#undef PCHECK
// CERES Clashes with logging symbols...
#include "ceres/ceres.h"
diff --git a/y2019/vision/server/BUILD b/y2019/vision/server/BUILD
index 3600194..a7126e4 100644
--- a/y2019/vision/server/BUILD
+++ b/y2019/vision/server/BUILD
@@ -1,6 +1,6 @@
load("//aos/seasocks:gen_embedded.bzl", "gen_embedded")
load("@com_google_protobuf//:protobuf.bzl", "cc_proto_library")
-load("//aos/downloader:downloader.bzl", "aos_downloader_dir")
+load("//frc971/downloader:downloader.bzl", "aos_downloader_dir")
load("@build_bazel_rules_typescript//:defs.bzl", "ts_library")
load("@build_bazel_rules_nodejs//:defs.bzl", "rollup_bundle")
@@ -49,15 +49,15 @@
":server_data_proto",
"//aos:init",
"//aos/containers:ring_buffer",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
"//aos/logging",
"//aos/seasocks:seasocks_logger",
"//aos/time",
"//frc971/control_loops:pose",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//third_party/seasocks",
"//y2019:constants",
- "//y2019/control_loops/drivetrain:camera_queue",
- "//y2019/control_loops/superstructure:superstructure_queue",
+ "//y2019/control_loops/drivetrain:camera_fbs",
+ "//y2019/control_loops/superstructure:superstructure_status_fbs",
],
)
diff --git a/y2019/vision/server/server.cc b/y2019/vision/server/server.cc
index e66a70b..91dc61a 100644
--- a/y2019/vision/server/server.cc
+++ b/y2019/vision/server/server.cc
@@ -8,11 +8,12 @@
#include <sstream>
#include "aos/containers/ring_buffer.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
#include "aos/seasocks/seasocks_logger.h"
#include "aos/time/time.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/pose.h"
#include "google/protobuf/util/json_util.h"
#include "internal/Embedded.h"
@@ -20,8 +21,8 @@
#include "seasocks/StringUtil.h"
#include "seasocks/WebSocket.h"
#include "y2019/constants.h"
-#include "y2019/control_loops/drivetrain/camera.q.h"
-#include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "y2019/control_loops/drivetrain/camera_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_status_generated.h"
#include "y2019/vision/server/server_data.pb.h"
namespace y2019 {
@@ -133,19 +134,21 @@
}
void DataThread(seasocks::Server *server, WebsocketHandler *websocket_handler) {
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ ::aos::ShmEventLoop event_loop(&config.message());
+
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher =
- event_loop
- .MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
- ".frc971.control_loops.drivetrain_queue.status");
+ event_loop.MakeFetcher<::frc971::control_loops::drivetrain::Status>(
+ "/drivetrain");
- ::aos::Fetcher<
- ::y2019::control_loops::superstructure::SuperstructureQueue::Status>
- superstructure_status_fetcher = event_loop.MakeFetcher<
- ::y2019::control_loops::superstructure::SuperstructureQueue::Status>(
- ".y2019.control_loops.superstructure.superstructure_queue.status");
+ ::aos::Fetcher<::y2019::control_loops::superstructure::Status>
+ superstructure_status_fetcher =
+ event_loop
+ .MakeFetcher<::y2019::control_loops::superstructure::Status>(
+ "/superstructure");
::std::array<LocalCameraFrame, 5> latest_frames;
::std::array<aos::monotonic_clock::time_point, 5> last_target_time;
@@ -155,7 +158,7 @@
::aos::RingBuffer<DrivetrainPosition, 200> drivetrain_log;
event_loop.MakeWatcher(
- ".y2019.control_loops.drivetrain.camera_frames",
+ "/drivetrain",
[websocket_handler, server, &latest_frames, &last_target_time,
&drivetrain_status_fetcher, &superstructure_status_fetcher,
&last_send_time, &drivetrain_log,
@@ -163,9 +166,9 @@
&camera_frames) {
while (drivetrain_status_fetcher.FetchNext()) {
DrivetrainPosition drivetrain_position{
- drivetrain_status_fetcher->sent_time,
- drivetrain_status_fetcher->x, drivetrain_status_fetcher->y,
- drivetrain_status_fetcher->theta};
+ drivetrain_status_fetcher.context().monotonic_sent_time,
+ drivetrain_status_fetcher->x(), drivetrain_status_fetcher->y(),
+ drivetrain_status_fetcher->theta()};
drivetrain_log.Push(drivetrain_position);
}
@@ -181,25 +184,26 @@
const auto &new_frame = camera_frames;
// TODO(james): Maybe we only want to fill out a new frame if it has
// targets or the saved target is > 0.1 sec old? Not sure, but for now
- if (new_frame.camera < latest_frames.size()) {
- latest_frames[new_frame.camera].capture_time =
+ if (new_frame.camera() < latest_frames.size()) {
+ latest_frames[new_frame.camera()].capture_time =
aos::monotonic_clock::time_point(
- chrono::nanoseconds(new_frame.timestamp));
- latest_frames[new_frame.camera].targets.clear();
- if (new_frame.num_targets > 0) {
- last_target_time[new_frame.camera] =
- latest_frames[new_frame.camera].capture_time;
+ chrono::nanoseconds(new_frame.timestamp()));
+ latest_frames[new_frame.camera()].targets.clear();
+ if (new_frame.has_targets() && new_frame.targets()->size() > 0) {
+ last_target_time[new_frame.camera()] =
+ latest_frames[new_frame.camera()].capture_time;
}
- for (int target = 0; target < new_frame.num_targets; ++target) {
- latest_frames[new_frame.camera].targets.emplace_back();
- const float heading = new_frame.targets[target].heading;
- const float distance = new_frame.targets[target].distance;
- latest_frames[new_frame.camera].targets.back().x =
+ for (const control_loops::drivetrain::CameraTarget *target :
+ *new_frame.targets()) {
+ latest_frames[new_frame.camera()].targets.emplace_back();
+ const float heading = target->heading();
+ const float distance = target->distance();
+ latest_frames[new_frame.camera()].targets.back().x =
::std::cos(heading) * distance;
- latest_frames[new_frame.camera].targets.back().y =
+ latest_frames[new_frame.camera()].targets.back().y =
::std::sin(heading) * distance;
- latest_frames[new_frame.camera].targets.back().theta =
- new_frame.targets[target].skew;
+ latest_frames[new_frame.camera()].targets.back().theta =
+ target->skew();
}
}
}
@@ -235,32 +239,33 @@
if (now > last_send_time + chrono::milliseconds(100)) {
last_send_time = now;
- debug_data.mutable_robot_pose()->set_x(drivetrain_status_fetcher->x);
- debug_data.mutable_robot_pose()->set_y(drivetrain_status_fetcher->y);
+ debug_data.mutable_robot_pose()->set_x(drivetrain_status_fetcher->x());
+ debug_data.mutable_robot_pose()->set_y(drivetrain_status_fetcher->y());
debug_data.mutable_robot_pose()->set_theta(
- drivetrain_status_fetcher->theta);
+ drivetrain_status_fetcher->theta());
{
LineFollowDebug *line_debug =
debug_data.mutable_line_follow_debug();
line_debug->set_frozen(
- drivetrain_status_fetcher->line_follow_logging.frozen);
+ drivetrain_status_fetcher->line_follow_logging()->frozen());
line_debug->set_have_target(
- drivetrain_status_fetcher->line_follow_logging.have_target);
+ drivetrain_status_fetcher->line_follow_logging()->have_target());
line_debug->mutable_goal_target()->set_x(
- drivetrain_status_fetcher->line_follow_logging.x);
+ drivetrain_status_fetcher->line_follow_logging()->x());
line_debug->mutable_goal_target()->set_y(
- drivetrain_status_fetcher->line_follow_logging.y);
+ drivetrain_status_fetcher->line_follow_logging()->y());
line_debug->mutable_goal_target()->set_theta(
- drivetrain_status_fetcher->line_follow_logging.theta);
+ drivetrain_status_fetcher->line_follow_logging()->theta());
}
Sensors *sensors = debug_data.mutable_sensors();
- sensors->set_wrist(superstructure_status_fetcher->wrist.position);
+ sensors->set_wrist(
+ superstructure_status_fetcher->wrist()->position());
sensors->set_elevator(
- superstructure_status_fetcher->elevator.position);
- sensors->set_intake(superstructure_status_fetcher->intake.position);
- sensors->set_stilts(superstructure_status_fetcher->stilts.position);
- sensors->set_has_piece(superstructure_status_fetcher->has_piece);
+ superstructure_status_fetcher->elevator()->position());
+ sensors->set_intake(superstructure_status_fetcher->intake()->position());
+ sensors->set_stilts(superstructure_status_fetcher->stilts()->position());
+ sensors->set_has_piece(superstructure_status_fetcher->has_piece());
::std::string json;
google::protobuf::util::MessageToJsonString(debug_data, &json);
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index 4f09da1..227d3ca 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -20,20 +20,20 @@
#undef ERROR
#include "aos/commonmath.h"
-#include "aos/events/event-loop.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/event_loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
#include "aos/make_unique.h"
-#include "aos/robot_state/robot_state.q.h"
+#include "aos/realtime.h"
+#include "aos/robot_state/robot_state_generated.h"
#include "aos/time/time.h"
#include "aos/util/log_interval.h"
#include "aos/util/phased_loop.h"
#include "aos/util/wrapping_counter.h"
#include "ctre/phoenix/motorcontrol/can/TalonSRX.h"
-#include "frc971/autonomous/auto.q.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/autonomous/auto_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
#include "frc971/wpilib/ADIS16448.h"
#include "frc971/wpilib/buffered_pcm.h"
#include "frc971/wpilib/buffered_solenoid.h"
@@ -41,24 +41,25 @@
#include "frc971/wpilib/drivetrain_writer.h"
#include "frc971/wpilib/encoder_and_potentiometer.h"
#include "frc971/wpilib/joystick_sender.h"
-#include "frc971/wpilib/logging.q.h"
+#include "frc971/wpilib/logging_generated.h"
#include "frc971/wpilib/loop_output_handler.h"
#include "frc971/wpilib/pdp_fetcher.h"
#include "frc971/wpilib/sensor_reader.h"
#include "frc971/wpilib/wpilib_robot_base.h"
#include "y2019/constants.h"
-#include "y2019/control_loops/drivetrain/camera.q.h"
-#include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "y2019/control_loops/drivetrain/camera_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_position_generated.h"
#include "y2019/jevois/spi.h"
-#include "y2019/status_light.q.h"
+#include "y2019/status_light_generated.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
-using ::y2019::control_loops::superstructure::SuperstructureQueue;
using ::y2019::constants::Values;
using ::aos::monotonic_clock;
+namespace superstructure = ::y2019::control_loops::superstructure;
namespace chrono = ::std::chrono;
using aos::make_unique;
@@ -133,15 +134,14 @@
: ::frc971::wpilib::SensorReader(event_loop),
auto_mode_sender_(
event_loop->MakeSender<::frc971::autonomous::AutonomousMode>(
- ".frc971.autonomous.auto_mode")),
+ "/aos")),
superstructure_position_sender_(
- event_loop->MakeSender<SuperstructureQueue::Position>(
- ".y2019.control_loops.superstructure.superstructure_queue."
- "position")),
+ event_loop->MakeSender<superstructure::Position>(
+ "/superstructure")),
drivetrain_position_sender_(
- event_loop->MakeSender<
- ::frc971::control_loops::DrivetrainQueue::Position>(
- ".frc971.control_loops.drivetrain_queue.position")) {
+ event_loop
+ ->MakeSender<::frc971::control_loops::drivetrain::Position>(
+ "/drivetrain")) {
// Set to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
@@ -233,79 +233,106 @@
void RunIteration() override {
{
- auto drivetrain_message = drivetrain_position_sender_.MakeMessage();
- drivetrain_message->left_encoder =
- drivetrain_translate(drivetrain_left_encoder_->GetRaw());
- drivetrain_message->left_speed =
- drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod());
+ auto builder = drivetrain_position_sender_.MakeBuilder();
+ frc971::control_loops::drivetrain::Position::Builder drivetrain_builder =
+ builder.MakeBuilder<frc971::control_loops::drivetrain::Position>();
+ drivetrain_builder.add_left_encoder(
+ drivetrain_translate(drivetrain_left_encoder_->GetRaw()));
+ drivetrain_builder.add_left_speed(
+ drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod()));
- drivetrain_message->right_encoder =
- -drivetrain_translate(drivetrain_right_encoder_->GetRaw());
- drivetrain_message->right_speed = -drivetrain_velocity_translate(
- drivetrain_right_encoder_->GetPeriod());
+ drivetrain_builder.add_right_encoder(
+ -drivetrain_translate(drivetrain_right_encoder_->GetRaw()));
+ drivetrain_builder.add_right_speed(-drivetrain_velocity_translate(
+ drivetrain_right_encoder_->GetPeriod()));
- drivetrain_message.Send();
+ builder.Send(drivetrain_builder.Finish());
}
const auto values = constants::GetValues();
{
- auto superstructure_message =
- superstructure_position_sender_.MakeMessage();
+ auto builder = superstructure_position_sender_.MakeBuilder();
// Elevator
- CopyPosition(elevator_encoder_, &superstructure_message->elevator,
+ frc971::PotAndAbsolutePositionT elevator;
+ CopyPosition(elevator_encoder_, &elevator,
Values::kElevatorEncoderCountsPerRevolution(),
Values::kElevatorEncoderRatio(), elevator_pot_translate,
false, values.elevator.potentiometer_offset);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> elevator_offset =
+ frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &elevator);
+
// Intake
- CopyPosition(intake_encoder_, &superstructure_message->intake_joint,
+ frc971::AbsolutePositionT intake_joint;
+ CopyPosition(intake_encoder_, &intake_joint,
Values::kIntakeEncoderCountsPerRevolution(),
Values::kIntakeEncoderRatio(), false);
+ flatbuffers::Offset<frc971::AbsolutePosition> intake_joint_offset =
+ frc971::AbsolutePosition::Pack(*builder.fbb(), &intake_joint);
// Wrist
- CopyPosition(wrist_encoder_, &superstructure_message->wrist,
+ frc971::PotAndAbsolutePositionT wrist;
+ CopyPosition(wrist_encoder_, &wrist,
Values::kWristEncoderCountsPerRevolution(),
Values::kWristEncoderRatio(), wrist_pot_translate, false,
values.wrist.potentiometer_offset);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> wrist_offset =
+ frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &wrist);
// Stilts
- CopyPosition(stilts_encoder_, &superstructure_message->stilts,
+ frc971::PotAndAbsolutePositionT stilts;
+ CopyPosition(stilts_encoder_, &stilts,
Values::kStiltsEncoderCountsPerRevolution(),
Values::kStiltsEncoderRatio(), stilts_pot_translate, false,
values.stilts.potentiometer_offset);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> stilts_offset =
+ frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &stilts);
+
+ superstructure::Position::Builder position_builder =
+ builder.MakeBuilder<superstructure::Position>();
+
+ position_builder.add_elevator(elevator_offset);
+ position_builder.add_intake_joint(intake_joint_offset);
+ position_builder.add_wrist(wrist_offset);
+ position_builder.add_stilts(stilts_offset);
// Suction
constexpr float kMinVoltage = 0.5;
constexpr float kMaxVoltage = 2.1;
- superstructure_message->suction_pressure =
+ position_builder.add_suction_pressure(
(vacuum_sensor_->GetVoltage() - kMinVoltage) /
- (kMaxVoltage - kMinVoltage);
+ (kMaxVoltage - kMinVoltage));
- superstructure_message->platform_left_detect =
- !platform_left_detect_->Get();
- superstructure_message->platform_right_detect =
- !platform_right_detect_->Get();
+ position_builder.add_platform_left_detect(!platform_left_detect_->Get());
+ position_builder.add_platform_right_detect(
+ !platform_right_detect_->Get());
- superstructure_message.Send();
+ builder.Send(position_builder.Finish());
}
{
- auto auto_mode_message = auto_mode_sender_.MakeMessage();
- auto_mode_message->mode = 0;
+ auto builder = auto_mode_sender_.MakeBuilder();
+
+ uint32_t mode = 0;
for (size_t i = 0; i < autonomous_modes_.size(); ++i) {
if (autonomous_modes_[i] && autonomous_modes_[i]->Get()) {
- auto_mode_message->mode |= 1 << i;
+ mode |= 1 << i;
}
}
- AOS_LOG_STRUCT(DEBUG, "auto mode", *auto_mode_message);
- auto_mode_message.Send();
+
+ auto auto_mode_builder =
+ builder.MakeBuilder<frc971::autonomous::AutonomousMode>();
+
+ auto_mode_builder.add_mode(mode);
+
+ builder.Send(auto_mode_builder.Finish());
}
}
private:
::aos::Sender<::frc971::autonomous::AutonomousMode> auto_mode_sender_;
- ::aos::Sender<SuperstructureQueue::Position> superstructure_position_sender_;
- ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Position>
+ ::aos::Sender<superstructure::Position> superstructure_position_sender_;
+ ::aos::Sender<::frc971::control_loops::drivetrain::Position>
drivetrain_position_sender_;
::frc971::wpilib::AbsoluteEncoderAndPotentiometer elevator_encoder_,
@@ -319,7 +346,6 @@
::std::array<::std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
::frc971::wpilib::AbsoluteEncoder intake_encoder_;
- // TODO(sabina): Add wrist and elevator hall effects.
};
class CameraReader {
@@ -363,7 +389,7 @@
to_teensy.camera_command = CameraCommand::kUsb;
} else if (activate_passthrough_ && !activate_passthrough_->Get()) {
to_teensy.camera_command = CameraCommand::kCameraPassthrough;
- } else if (camera_log_fetcher_.get() && camera_log_fetcher_->log) {
+ } else if (camera_log_fetcher_.get() && camera_log_fetcher_->log()) {
to_teensy.camera_command = CameraCommand::kLog;
} else {
to_teensy.camera_command = CameraCommand::kNormal;
@@ -388,25 +414,49 @@
return;
}
- const auto now = aos::monotonic_clock::now();
+ const aos::monotonic_clock::time_point now = aos::monotonic_clock::now();
for (const auto &received : unpacked->frames) {
- auto to_send = camera_frame_sender_.MakeMessage();
+ auto builder = camera_frame_sender_.MakeBuilder();
+
+ std::array<
+ flatbuffers::Offset<y2019::control_loops::drivetrain::CameraTarget>,
+ 3>
+ targets;
+
+ for (size_t i = 0; i < received.targets.size(); ++i) {
+ y2019::control_loops::drivetrain::CameraTarget::Builder
+ camera_target_builder = builder.MakeBuilder<
+ y2019::control_loops::drivetrain::CameraTarget>();
+
+ camera_target_builder.add_distance(received.targets[i].distance);
+ camera_target_builder.add_height(received.targets[i].height);
+ camera_target_builder.add_heading(received.targets[i].heading);
+ camera_target_builder.add_skew(received.targets[i].skew);
+
+ targets[i] = camera_target_builder.Finish();
+ }
+
+ flatbuffers::Offset<flatbuffers::Vector<
+ flatbuffers::Offset<y2019::control_loops::drivetrain::CameraTarget>>>
+ targets_offset = builder.fbb()->CreateVector(targets.begin(),
+ received.targets.size());
+
+ y2019::control_loops::drivetrain::CameraFrame::Builder
+ camera_frame_builder =
+ builder
+ .MakeBuilder<y2019::control_loops::drivetrain::CameraFrame>();
+
+ camera_frame_builder.add_targets(targets_offset);
+
// Add an extra 10ms delay to account for unmodeled delays that Austin
// thinks exists.
- to_send->timestamp =
+ camera_frame_builder.add_timestamp(
std::chrono::nanoseconds(
(now - received.age - ::std::chrono::milliseconds(10))
- .time_since_epoch()).count();
- to_send->num_targets = received.targets.size();
- for (size_t i = 0; i < received.targets.size(); ++i) {
- to_send->targets[i].distance = received.targets[i].distance;
- to_send->targets[i].height = received.targets[i].height;
- to_send->targets[i].heading = received.targets[i].heading;
- to_send->targets[i].skew = received.targets[i].skew;
- }
- to_send->camera = received.camera_index;
- AOS_LOG_STRUCT(DEBUG, "camera_frames", *to_send);
- to_send.Send();
+ .time_since_epoch())
+ .count());
+ camera_frame_builder.add_camera(received.camera_index);
+ builder.Send(camera_frame_builder.Finish());
}
if (dummy_spi_) {
@@ -458,14 +508,13 @@
};
class SuperstructureWriter
- : public ::frc971::wpilib::LoopOutputHandler<SuperstructureQueue::Output> {
+ : public ::frc971::wpilib::LoopOutputHandler<superstructure::Output> {
public:
SuperstructureWriter(::aos::EventLoop *event_loop)
- : ::frc971::wpilib::LoopOutputHandler<SuperstructureQueue::Output>(
- event_loop,
- ".y2019.control_loops.superstructure.superstructure_queue.output"),
+ : ::frc971::wpilib::LoopOutputHandler<superstructure::Output>(
+ event_loop, "/superstructure"),
robot_state_fetcher_(
- event_loop->MakeFetcher<::aos::RobotState>(".aos.robot_state")) {}
+ event_loop->MakeFetcher<::aos::RobotState>("/aos")) {}
void set_elevator_victor(::std::unique_ptr<::frc::VictorSP> t) {
elevator_victor_ = ::std::move(t);
@@ -488,28 +537,27 @@
}
private:
- void Write(const SuperstructureQueue::Output &output) override {
- AOS_LOG_STRUCT(DEBUG, "will output", output);
- elevator_victor_->SetSpeed(::aos::Clip(output.elevator_voltage,
+ void Write(const superstructure::Output &output) override {
+ elevator_victor_->SetSpeed(::aos::Clip(output.elevator_voltage(),
-kMaxBringupPower,
kMaxBringupPower) /
12.0);
- intake_victor_->SetSpeed(::aos::Clip(output.intake_joint_voltage,
+ intake_victor_->SetSpeed(::aos::Clip(output.intake_joint_voltage(),
-kMaxBringupPower, kMaxBringupPower) /
12.0);
- wrist_victor_->SetSpeed(::aos::Clip(-output.wrist_voltage,
+ wrist_victor_->SetSpeed(::aos::Clip(-output.wrist_voltage(),
-kMaxBringupPower, kMaxBringupPower) /
12.0);
- stilts_victor_->SetSpeed(::aos::Clip(output.stilts_voltage,
+ stilts_victor_->SetSpeed(::aos::Clip(output.stilts_voltage(),
-kMaxBringupPower, kMaxBringupPower) /
12.0);
robot_state_fetcher_.Fetch();
const double battery_voltage = robot_state_fetcher_.get()
- ? robot_state_fetcher_->voltage_battery
+ ? robot_state_fetcher_->voltage_battery()
: 12.0;
// Throw a fast low pass filter on the battery voltage so we don't respond
@@ -518,7 +566,7 @@
0.5 * filtered_battery_voltage_ + 0.5 * battery_voltage;
suction_victor_->SetSpeed(::aos::Clip(
- output.pump_voltage / filtered_battery_voltage_, -1.0, 1.0));
+ output.pump_voltage() / filtered_battery_voltage_, -1.0, 1.0));
}
void Stop() override {
@@ -543,11 +591,12 @@
public:
SolenoidWriter(::aos::EventLoop *event_loop)
: event_loop_(event_loop),
- superstructure_fetcher_(event_loop->MakeFetcher<
- SuperstructureQueue::Output>(
- ".y2019.control_loops.superstructure.superstructure_queue.output")),
- status_light_fetcher_(event_loop->MakeFetcher<::y2019::StatusLight>(
- ".y2019.status_light")) {
+ superstructure_fetcher_(
+ event_loop->MakeFetcher<superstructure::Output>("/superstructure")),
+ status_light_fetcher_(
+ event_loop->MakeFetcher<::y2019::StatusLight>("/superstructure")),
+ pneumatics_to_log_sender_(
+ event_loop->MakeSender<::frc971::wpilib::PneumaticsToLog>("/aos")) {
::aos::SetCurrentThreadName("Solenoids");
::aos::SetCurrentThreadRealtimePriority(27);
@@ -580,36 +629,40 @@
{
superstructure_fetcher_.Fetch();
if (superstructure_fetcher_.get()) {
- AOS_LOG_STRUCT(DEBUG, "solenoids", *superstructure_fetcher_);
-
- big_suction_cup0_->Set(!superstructure_fetcher_->intake_suction_bottom);
- big_suction_cup1_->Set(!superstructure_fetcher_->intake_suction_bottom);
- small_suction_cup0_->Set(superstructure_fetcher_->intake_suction_top);
- small_suction_cup1_->Set(superstructure_fetcher_->intake_suction_top);
+ big_suction_cup0_->Set(
+ !superstructure_fetcher_->intake_suction_bottom());
+ big_suction_cup1_->Set(
+ !superstructure_fetcher_->intake_suction_bottom());
+ small_suction_cup0_->Set(superstructure_fetcher_->intake_suction_top());
+ small_suction_cup1_->Set(superstructure_fetcher_->intake_suction_top());
intake_rollers_talon_->Set(
ctre::phoenix::motorcontrol::ControlMode::PercentOutput,
- ::aos::Clip(superstructure_fetcher_->intake_roller_voltage,
+ ::aos::Clip(superstructure_fetcher_->intake_roller_voltage(),
-kMaxBringupPower, kMaxBringupPower) /
12.0);
}
}
{
- ::frc971::wpilib::PneumaticsToLog to_log;
+ auto builder = pneumatics_to_log_sender_.MakeBuilder();
+
+ ::frc971::wpilib::PneumaticsToLog::Builder to_log_builder =
+ builder.MakeBuilder<frc971::wpilib::PneumaticsToLog>();
pcm_.Flush();
- to_log.read_solenoids = pcm_.GetAll();
- AOS_LOG_STRUCT(DEBUG, "pneumatics info", to_log);
+ to_log_builder.add_read_solenoids(pcm_.GetAll());
+ builder.Send(to_log_builder.Finish());
}
status_light_fetcher_.Fetch();
// If we don't have a light request (or it's an old one), we are borked.
// Flash the red light slowly.
+ StatusLightT color;
if (!status_light_fetcher_.get() ||
- status_light_fetcher_.get()->sent_time + chrono::milliseconds(100) <
+ status_light_fetcher_.context().monotonic_sent_time +
+ chrono::milliseconds(100) <
event_loop_->monotonic_now()) {
- StatusLight color;
color.red = 0.0;
color.green = 0.0;
color.blue = 0.0;
@@ -623,15 +676,13 @@
light_flash_ = 0;
}
- AOS_LOG_STRUCT(DEBUG, "color", color);
- SetColor(color);
} else {
- AOS_LOG_STRUCT(DEBUG, "color", *status_light_fetcher_.get());
- SetColor(*status_light_fetcher_.get());
+ status_light_fetcher_->UnPackTo(&color);
}
+ SetColor(color);
}
- void SetColor(const StatusLight &status_light) {
+ void SetColor(const StatusLightT status_light) {
// Save CAN bandwidth and CPU at the cost of RT. Only change the light when
// it actually changes. This is pretty low priority anyways.
static int time_since_last_send = 0;
@@ -669,11 +720,12 @@
::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonSRX>
intake_rollers_talon_;
- ::aos::Fetcher<
- ::y2019::control_loops::superstructure::SuperstructureQueue::Output>
+ ::aos::Fetcher<::y2019::control_loops::superstructure::Output>
superstructure_fetcher_;
::aos::Fetcher<::y2019::StatusLight> status_light_fetcher_;
+ aos::Sender<::frc971::wpilib::PneumaticsToLog> pneumatics_to_log_sender_;
+
::ctre::phoenix::CANifier canifier_{0};
double last_red_ = -1.0;
@@ -691,19 +743,22 @@
}
void Run() override {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
// Thread 1.
- ::aos::ShmEventLoop joystick_sender_event_loop;
+ ::aos::ShmEventLoop joystick_sender_event_loop(&config.message());
::frc971::wpilib::JoystickSender joystick_sender(
&joystick_sender_event_loop);
AddLoop(&joystick_sender_event_loop);
// Thread 2.
- ::aos::ShmEventLoop pdp_fetcher_event_loop;
+ ::aos::ShmEventLoop pdp_fetcher_event_loop(&config.message());
::frc971::wpilib::PDPFetcher pdp_fetcher(&pdp_fetcher_event_loop);
AddLoop(&pdp_fetcher_event_loop);
// Thread 3.
- ::aos::ShmEventLoop sensor_reader_event_loop;
+ ::aos::ShmEventLoop sensor_reader_event_loop(&config.message());
SensorReader sensor_reader(&sensor_reader_event_loop);
sensor_reader.set_drivetrain_left_encoder(make_encoder(0));
sensor_reader.set_drivetrain_right_encoder(make_encoder(1));
@@ -734,7 +789,7 @@
AddLoop(&sensor_reader_event_loop);
// Thread 4.
- ::aos::ShmEventLoop imu_event_loop;
+ ::aos::ShmEventLoop imu_event_loop(&config.message());
CameraReader camera_reader(&imu_event_loop);
frc::SPI camera_spi(frc::SPI::Port::kOnboardCS3);
camera_reader.set_spi(&camera_spi);
@@ -757,7 +812,7 @@
// variety so all the Victors are written as SPs.
// Thread 5.
- ::aos::ShmEventLoop output_event_loop;
+ ::aos::ShmEventLoop output_event_loop(&config.message());
::frc971::wpilib::DrivetrainWriter drivetrain_writer(&output_event_loop);
drivetrain_writer.set_left_controller0(
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(0)), true);
@@ -779,7 +834,7 @@
AddLoop(&output_event_loop);
// Thread 6.
- ::aos::ShmEventLoop solenoid_writer_event_loop;
+ ::aos::ShmEventLoop solenoid_writer_event_loop(&config.message());
SolenoidWriter solenoid_writer(&solenoid_writer_event_loop);
solenoid_writer.set_intake_roller_talon(
make_unique<::ctre::phoenix::motorcontrol::can::TalonSRX>(10));
diff --git a/y2019/y2019.json b/y2019/y2019.json
new file mode 100644
index 0000000..c32db3f
--- /dev/null
+++ b/y2019/y2019.json
@@ -0,0 +1,49 @@
+{
+ "channels":
+ [
+ {
+ "name": "/superstructure",
+ "type": "y2019.control_loops.superstructure.Goal",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2019.control_loops.superstructure.Status",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2019.control_loops.superstructure.Output",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2019.control_loops.superstructure.Position",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2019.StatusLight",
+ "frequency": 200
+ },
+ {
+ "name": "/drivetrain",
+ "type": "y2019.control_loops.drivetrain.TargetSelectorHint",
+ "frequency": 200
+ },
+ {
+ "name": "/drivetrain",
+ "type": "y2019.control_loops.drivetrain.CameraFrame",
+ "frequency": 200
+ }
+ ],
+ "applications": [
+ {
+ "name": "drivetrain"
+ }
+ ],
+ "imports": [
+ "../aos/robot_state/robot_state_config.json",
+ "../frc971/control_loops/drivetrain/drivetrain_config.json"
+ ]
+}