Convert aos over to flatbuffers
Everything builds, and all the tests pass. I suspect that some entries
are missing from the config files, but those will be found pretty
quickly on startup.
There is no logging or live introspection of queue messages.
Change-Id: I496ee01ed68f202c7851bed7e8786cee30df29f5
diff --git a/y2018/BUILD b/y2018/BUILD
index d7cc2fd..94fd844 100644
--- a/y2018/BUILD
+++ b/y2018/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(
start_binaries = [
@@ -30,11 +31,12 @@
"//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",
"//y2018/control_loops/drivetrain:drivetrain_base",
- "//y2018/control_loops/superstructure:superstructure_queue",
+ "//y2018/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2018/control_loops/superstructure:superstructure_position_fbs",
+ "//y2018/control_loops/superstructure:superstructure_status_fbs",
"//y2018/control_loops/superstructure/arm:generated_graph",
],
)
@@ -67,21 +69,21 @@
],
restricted_to = ["//tools:roborio"],
deps = [
- ":status_light",
+ ":status_light_fbs",
"//aos:init",
"//aos:make_unique",
"//aos:math",
"//aos/controls:control_loop",
"//aos/logging",
- "//aos/logging:queue_logging",
- "//aos/robot_state",
+ "//aos/robot_state:robot_state_fbs",
"//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/control_loops/drivetrain:drivetrain_status_fbs",
"//frc971/wpilib:ADIS16448",
"//frc971/wpilib:buffered_pcm",
"//frc971/wpilib:dma",
@@ -89,7 +91,7 @@
"//frc971/wpilib:drivetrain_writer",
"//frc971/wpilib:encoder_and_potentiometer",
"//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",
@@ -97,17 +99,37 @@
"//third_party:phoenix",
"//third_party:wpilib",
"//y2018:constants",
- "//y2018/control_loops/superstructure:superstructure_queue",
- "//y2018/vision:vision_queue",
+ "//y2018/control_loops/superstructure:superstructure_output_fbs",
+ "//y2018/control_loops/superstructure:superstructure_position_fbs",
+ "//y2018/vision:vision_fbs",
],
)
-queue_library(
- name = "status_light",
+flatbuffer_cc_library(
+ name = "status_light_fbs",
srcs = [
- "status_light.q",
+ "status_light.fbs",
+ ],
+ gen_reflections = 1,
+ visibility = ["//visibility:public"],
+)
+
+aos_config(
+ name = "config",
+ src = "y2018.json",
+ flatbuffers = [
+ ":status_light_fbs",
+ "//y2018/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2018/control_loops/superstructure:superstructure_output_fbs",
+ "//y2018/control_loops/superstructure:superstructure_position_fbs",
+ "//y2018/control_loops/superstructure:superstructure_status_fbs",
+ "//y2018/vision:vision_fbs",
],
visibility = ["//visibility:public"],
+ deps = [
+ "//aos/robot_state:config",
+ "//frc971/control_loops/drivetrain:config",
+ ],
)
cc_proto_library(
diff --git a/y2018/actors/BUILD b/y2018/actors/BUILD
index 7d97f66..7a95419 100644
--- a/y2018/actors/BUILD
+++ b/y2018/actors/BUILD
@@ -8,14 +8,14 @@
],
deps = [
"//aos/actions:action_lib",
- "//aos/events:event-loop",
+ "//aos/events:event_loop",
"//aos/logging",
"//aos/util:phased_loop",
"//frc971/autonomous:base_autonomous_actor",
"//frc971/control_loops/drivetrain:drivetrain_config",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
"//y2018/control_loops/drivetrain:drivetrain_base",
- "//y2018/control_loops/superstructure:superstructure_queue",
+ "//y2018/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2018/control_loops/superstructure:superstructure_status_fbs",
"//y2018/control_loops/superstructure/arm:generated_graph",
],
)
@@ -29,7 +29,6 @@
deps = [
":autonomous_action_lib",
"//aos:init",
- "//aos/events:shm-event-loop",
- "//frc971/autonomous:auto_queue",
+ "//aos/events:shm_event_loop",
],
)
diff --git a/y2018/actors/autonomous_actor.cc b/y2018/actors/autonomous_actor.cc
index dd94083..89082df 100644
--- a/y2018/actors/autonomous_actor.cc
+++ b/y2018/actors/autonomous_actor.cc
@@ -8,12 +8,11 @@
#include "aos/logging/logging.h"
#include "aos/util/phased_loop.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2018/control_loops/drivetrain/drivetrain_base.h"
namespace y2018 {
namespace actors {
-using ::frc971::ProfileParameters;
+using ::frc971::ProfileParametersT;
using ::aos::monotonic_clock;
namespace chrono = ::std::chrono;
@@ -23,25 +22,34 @@
namespace arm = ::y2018::control_loops::superstructure::arm;
-const ProfileParameters kFinalSwitchDrive = {0.5, 1.5};
-const ProfileParameters kDrive = {5.0, 2.5};
-const ProfileParameters kThirdBoxDrive = {3.0, 2.5};
-const ProfileParameters kSlowDrive = {1.5, 2.5};
-const ProfileParameters kScaleTurnDrive = {3.0, 2.5};
-const ProfileParameters kFarSwitchTurnDrive = {2.0, 2.5};
-const ProfileParameters kFarScaleFinalTurnDrive = kFarSwitchTurnDrive;
-const ProfileParameters kTurn = {4.0, 2.0};
-const ProfileParameters kSweepingTurn = {5.0, 7.0};
-const ProfileParameters kFarScaleSweepingTurn = kSweepingTurn;
-const ProfileParameters kFastTurn = {5.0, 7.0};
-const ProfileParameters kReallyFastTurn = {1.5, 9.0};
+ProfileParametersT MakeProfileParameters(float max_velocity,
+ float max_acceleration) {
+ ProfileParametersT result;
+ result.max_velocity = max_velocity;
+ result.max_acceleration = max_acceleration;
+ return result;
+}
-const ProfileParameters kThirdBoxSlowBackup = {0.35, 1.5};
-const ProfileParameters kThirdBoxSlowTurn = {1.5, 4.0};
+const ProfileParametersT kFinalSwitchDrive = MakeProfileParameters(0.5, 1.5);
+const ProfileParametersT kDrive = MakeProfileParameters(5.0, 2.5);
+const ProfileParametersT kThirdBoxDrive = MakeProfileParameters(3.0, 2.5);
+const ProfileParametersT kSlowDrive = MakeProfileParameters(1.5, 2.5);
+const ProfileParametersT kScaleTurnDrive = MakeProfileParameters(3.0, 2.5);
+const ProfileParametersT kFarSwitchTurnDrive = MakeProfileParameters(2.0, 2.5);
+const ProfileParametersT kFarScaleFinalTurnDrive = kFarSwitchTurnDrive;
+const ProfileParametersT kTurn = MakeProfileParameters(4.0, 2.0);
+const ProfileParametersT kSweepingTurn = MakeProfileParameters(5.0, 7.0);
+const ProfileParametersT kFarScaleSweepingTurn = kSweepingTurn;
+const ProfileParametersT kFastTurn = MakeProfileParameters(5.0, 7.0);
+const ProfileParametersT kReallyFastTurn = MakeProfileParameters(1.5, 9.0);
-const ProfileParameters kThirdBoxPlaceDrive = {4.0, 2.0};
+const ProfileParametersT kThirdBoxSlowBackup = MakeProfileParameters(0.35, 1.5);
+const ProfileParametersT kThirdBoxSlowTurn = MakeProfileParameters(1.5, 4.0);
-const ProfileParameters kFinalFrontFarSwitchDrive = {2.0, 2.0};
+const ProfileParametersT kThirdBoxPlaceDrive = MakeProfileParameters(4.0, 2.0);
+
+const ProfileParametersT kFinalFrontFarSwitchDrive =
+ MakeProfileParameters(2.0, 2.0);
} // namespace
@@ -49,24 +57,23 @@
: frc971::autonomous::BaseAutonomousActor(
event_loop, control_loops::drivetrain::GetDrivetrainConfig()),
superstructure_goal_sender_(
- event_loop
- ->MakeSender<::y2018::control_loops::SuperstructureQueue::Goal>(
- ".frc971.control_loops.superstructure_queue.goal")),
+ event_loop->MakeSender<::y2018::control_loops::superstructure::Goal>(
+ "/superstructure")),
superstructure_status_fetcher_(
- event_loop->MakeFetcher<
- ::y2018::control_loops::SuperstructureQueue::Status>(
- ".frc971.control_loops.superstructure_queue.status")) {}
+ event_loop
+ ->MakeFetcher<::y2018::control_loops::superstructure::Status>(
+ "/superstructure")) {}
bool AutonomousActor::RunAction(
- const ::frc971::autonomous::AutonomousActionParams ¶ms) {
+ const ::frc971::autonomous::AutonomousActionParams *params) {
const monotonic_clock::time_point start_time = monotonic_now();
AOS_LOG(INFO, "Starting autonomous action with mode %" PRId32 "\n",
- params.mode);
+ params->mode());
Reset();
// Switch
/*
- switch (params.mode) {
+ switch (params->mode()) {
case 0:
case 2: {
if (FarSwitch(start_time, true)) return true;
@@ -79,7 +86,7 @@
}
*/
// Scale
- switch (params.mode) {
+ switch (params->mode()) {
case 0:
case 1: {
if (FarScale(start_time)) return true;
@@ -91,7 +98,7 @@
} break;
}
/*
- switch (params.mode) {
+ switch (params->mode()) {
case 1: {
if (FarScale(start_time)) return true;
//if (CloseSwitch(start_time)) return true;
diff --git a/y2018/actors/autonomous_actor.h b/y2018/actors/autonomous_actor.h
index 3ef7677..014459a 100644
--- a/y2018/actors/autonomous_actor.h
+++ b/y2018/actors/autonomous_actor.h
@@ -6,12 +6,13 @@
#include "aos/actions/actions.h"
#include "aos/actions/actor.h"
-#include "aos/events/event-loop.h"
+#include "aos/events/event_loop.h"
#include "frc971/autonomous/base_autonomous_actor.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "y2018/control_loops/superstructure/arm/generated_graph.h"
-#include "y2018/control_loops/superstructure/superstructure.q.h"
+#include "y2018/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2018/control_loops/superstructure/superstructure_status_generated.h"
namespace y2018 {
namespace actors {
@@ -21,7 +22,7 @@
explicit AutonomousActor(::aos::EventLoop *event_loop);
bool RunAction(
- const ::frc971::autonomous::AutonomousActionParams ¶ms) override;
+ const ::frc971::autonomous::AutonomousActionParams *params) override;
private:
void Reset() {
@@ -40,9 +41,9 @@
SendSuperstructureGoal();
}
- ::aos::Sender<::y2018::control_loops::SuperstructureQueue::Goal>
+ ::aos::Sender<::y2018::control_loops::superstructure::Goal>
superstructure_goal_sender_;
- ::aos::Fetcher<::y2018::control_loops::SuperstructureQueue::Status>
+ ::aos::Fetcher<::y2018::control_loops::superstructure::Status>
superstructure_status_fetcher_;
double roller_voltage_ = 0.0;
@@ -82,19 +83,29 @@
}
void SendSuperstructureGoal() {
- auto new_superstructure_goal = superstructure_goal_sender_.MakeMessage();
- new_superstructure_goal->intake.roller_voltage = roller_voltage_;
- new_superstructure_goal->intake.left_intake_angle = left_intake_angle_;
- new_superstructure_goal->intake.right_intake_angle = right_intake_angle_;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ control_loops::superstructure::IntakeGoal::Builder intake_goal_builder =
+ builder.MakeBuilder<control_loops::superstructure::IntakeGoal>();
+ intake_goal_builder.add_roller_voltage(roller_voltage_);
+ intake_goal_builder.add_left_intake_angle(left_intake_angle_);
+ intake_goal_builder.add_right_intake_angle(right_intake_angle_);
- new_superstructure_goal->arm_goal_position = arm_goal_position_;
- new_superstructure_goal->grab_box = grab_box_;
- new_superstructure_goal->open_claw = open_claw_;
- new_superstructure_goal->close_claw = close_claw_;
- new_superstructure_goal->deploy_fork = deploy_fork_;
- new_superstructure_goal->trajectory_override = false;
+ flatbuffers::Offset<control_loops::superstructure::IntakeGoal>
+ intake_offset = intake_goal_builder.Finish();
- if (!new_superstructure_goal.Send()) {
+ control_loops::superstructure::Goal::Builder superstructure_builder =
+ builder.MakeBuilder<control_loops::superstructure::Goal>();
+
+ superstructure_builder.add_intake(intake_offset);
+
+ superstructure_builder.add_arm_goal_position(arm_goal_position_);
+ superstructure_builder.add_grab_box(grab_box_);
+ superstructure_builder.add_open_claw(open_claw_);
+ superstructure_builder.add_close_claw(close_claw_);
+ superstructure_builder.add_deploy_fork(deploy_fork_);
+ superstructure_builder.add_trajectory_override(false);
+
+ if (!builder.Send(superstructure_builder.Finish())) {
AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
}
}
@@ -129,17 +140,17 @@
superstructure_status_fetcher_.get()) {
const double left_profile_error =
(initial_drivetrain_.left -
- drivetrain_status_fetcher_->profiled_left_position_goal);
+ drivetrain_status_fetcher_->profiled_left_position_goal());
const double right_profile_error =
(initial_drivetrain_.right -
- drivetrain_status_fetcher_->profiled_right_position_goal);
+ drivetrain_status_fetcher_->profiled_right_position_goal());
const double left_error =
(initial_drivetrain_.left -
- drivetrain_status_fetcher_->estimated_left_position);
+ drivetrain_status_fetcher_->estimated_left_position());
const double right_error =
(initial_drivetrain_.right -
- drivetrain_status_fetcher_->estimated_right_position);
+ drivetrain_status_fetcher_->estimated_right_position());
const double profile_distance_to_go =
(left_profile_error + right_profile_error) / 2.0;
@@ -147,12 +158,12 @@
const double distance_to_go = (left_error + right_error) / 2.0;
// Check superstructure first.
- if (superstructure_status_fetcher_->arm.current_node ==
+ if (superstructure_status_fetcher_->arm()->current_node() ==
arm_goal_position_ &&
- superstructure_status_fetcher_->arm.path_distance_to_go <
+ superstructure_status_fetcher_->arm()->path_distance_to_go() <
arm_threshold) {
AOS_LOG(INFO, "Arm finished first: %f, drivetrain %f distance\n",
- superstructure_status_fetcher_->arm.path_distance_to_go,
+ superstructure_status_fetcher_->arm()->path_distance_to_go(),
::std::abs(distance_to_go));
return true;
}
@@ -163,7 +174,7 @@
::std::abs(distance_to_go) < drive_threshold + kPositionTolerance) {
AOS_LOG(INFO,
"Drivetrain finished first: arm %f, drivetrain %f distance\n",
- superstructure_status_fetcher_->arm.path_distance_to_go,
+ superstructure_status_fetcher_->arm()->path_distance_to_go(),
::std::abs(distance_to_go));
return true;
}
@@ -183,9 +194,9 @@
superstructure_status_fetcher_.Fetch();
if (superstructure_status_fetcher_.get()) {
- if (superstructure_status_fetcher_->arm.current_node ==
+ if (superstructure_status_fetcher_->arm()->current_node() ==
arm_goal_position_ &&
- superstructure_status_fetcher_->arm.path_distance_to_go <
+ superstructure_status_fetcher_->arm()->path_distance_to_go() <
threshold) {
return true;
}
@@ -205,7 +216,7 @@
superstructure_status_fetcher_.Fetch();
if (superstructure_status_fetcher_.get()) {
- if (superstructure_status_fetcher_->arm.grab_state == 4) {
+ if (superstructure_status_fetcher_->arm()->grab_state() == 4) {
return true;
}
}
diff --git a/y2018/actors/autonomous_actor_main.cc b/y2018/actors/autonomous_actor_main.cc
index 6193d48..76fae2e 100644
--- a/y2018/actors/autonomous_actor_main.cc
+++ b/y2018/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 "y2018/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());
::y2018::actors::AutonomousActor autonomous(&event_loop);
event_loop.Run();
diff --git a/y2018/control_loops/drivetrain/BUILD b/y2018/control_loops/drivetrain/BUILD
index 1824196..4d6a6a1 100644
--- a/y2018/control_loops/drivetrain/BUILD
+++ b/y2018/control_loops/drivetrain/BUILD
@@ -1,5 +1,3 @@
-load("//aos/build:queues.bzl", "queue_library")
-
genrule(
name = "genrule_drivetrain",
outs = [
diff --git a/y2018/control_loops/drivetrain/drivetrain_main.cc b/y2018/control_loops/drivetrain/drivetrain_main.cc
index 1bb1dc0..e5c68c5 100644
--- a/y2018/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2018/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 "y2018/control_loops/drivetrain/drivetrain_base.h"
@@ -9,7 +9,10 @@
int main() {
::aos::InitNRT(true);
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
&event_loop, ::y2018::control_loops::drivetrain::GetDrivetrainConfig());
DrivetrainLoop drivetrain(
diff --git a/y2018/control_loops/python/BUILD b/y2018/control_loops/python/BUILD
index af158de..5690b08 100644
--- a/y2018/control_loops/python/BUILD
+++ b/y2018/control_loops/python/BUILD
@@ -146,8 +146,8 @@
"arm_bounds.h",
],
deps = [
- "//third_party/eigen",
"@cgal_repo//:cgal",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -163,7 +163,6 @@
],
)
-
py_binary(
name = "graph_edit",
srcs = [
@@ -175,9 +174,9 @@
restricted_to = ["//tools:k8"],
srcs_version = "PY3",
deps = [
+ ":python_init",
"//frc971/control_loops/python:basic_window",
"//frc971/control_loops/python:color",
- ":python_init",
"@python_gtk",
],
)
diff --git a/y2018/control_loops/superstructure/BUILD b/y2018/control_loops/superstructure/BUILD
index 5dabd1b..4e97571 100644
--- a/y2018/control_loops/superstructure/BUILD
+++ b/y2018/control_loops/superstructure/BUILD
@@ -1,18 +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:queues",
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
],
)
+flatbuffer_cc_library(
+ name = "superstructure_position_fbs",
+ srcs = [
+ "superstructure_position.fbs",
+ ],
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "superstructure_status_fbs",
+ srcs = [
+ "superstructure_status.fbs",
+ ],
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "superstructure_output_fbs",
+ srcs = [
+ "superstructure_output.fbs",
+ ],
+ gen_reflections = 1,
+)
+
cc_library(
name = "superstructure_lib",
srcs = [
@@ -22,16 +52,19 @@
"superstructure.h",
],
deps = [
- ":superstructure_queue",
+ ":superstructure_goal_fbs",
+ ":superstructure_output_fbs",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
"//aos/controls:control_loop",
- "//aos/events:event-loop",
- "//frc971/control_loops:queues",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//aos/events:event_loop",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_output_fbs",
"//y2018:constants",
- "//y2018:status_light",
+ "//y2018:status_light_fbs",
"//y2018/control_loops/superstructure/arm",
"//y2018/control_loops/superstructure/intake",
- "//y2018/vision:vision_queue",
+ "//y2018/vision:vision_fbs",
],
)
@@ -41,12 +74,15 @@
srcs = [
"superstructure_lib_test.cc",
],
+ data = ["//y2018:config.json"],
shard_count = 5,
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",
@@ -63,7 +99,6 @@
],
deps = [
":superstructure_lib",
- ":superstructure_queue",
"//aos:init",
],
)
diff --git a/y2018/control_loops/superstructure/arm/BUILD b/y2018/control_loops/superstructure/arm/BUILD
index e8af2b9..bf3c306 100644
--- a/y2018/control_loops/superstructure/arm/BUILD
+++ b/y2018/control_loops/superstructure/arm/BUILD
@@ -12,7 +12,7 @@
"//aos/logging",
"//frc971/control_loops:dlqr",
"//frc971/control_loops:jacobian",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -27,7 +27,7 @@
":ekf",
":trajectory",
"//aos/testing:googletest",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -42,8 +42,8 @@
visibility = ["//visibility:public"],
deps = [
"//frc971/control_loops:runge_kutta",
- "//third_party/eigen",
"@com_github_gflags_gflags//:gflags",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -77,9 +77,9 @@
":ekf",
":generated_graph",
":trajectory",
- "//third_party/eigen",
"//third_party/matplotlib-cpp",
"@com_github_gflags_gflags//:gflags",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -95,7 +95,7 @@
deps = [
":dynamics",
"//frc971/control_loops:jacobian",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -129,10 +129,10 @@
":generated_graph",
":graph",
":trajectory",
- "//aos/logging:queue_logging",
"//frc971/zeroing",
"//y2018:constants",
- "//y2018/control_loops/superstructure:superstructure_queue",
+ "//y2018/control_loops/superstructure:superstructure_position_fbs",
+ "//y2018/control_loops/superstructure:superstructure_status_fbs",
],
)
diff --git a/y2018/control_loops/superstructure/arm/arm.cc b/y2018/control_loops/superstructure/arm/arm.cc
index 2ae1996..1f904e2 100644
--- a/y2018/control_loops/superstructure/arm/arm.cc
+++ b/y2018/control_loops/superstructure/arm/arm.cc
@@ -4,7 +4,6 @@
#include <iostream>
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
#include "y2018/constants.h"
#include "y2018/control_loops/superstructure/arm/demo_path.h"
#include "y2018/control_loops/superstructure/arm/dynamics.h"
@@ -44,15 +43,15 @@
void Arm::Reset() { state_ = State::UNINITIALIZED; }
-void Arm::Iterate(const ::aos::monotonic_clock::time_point monotonic_now,
- const uint32_t *unsafe_goal, bool grab_box, bool open_claw,
- bool close_claw, const control_loops::ArmPosition *position,
- const bool claw_beambreak_triggered,
- const bool box_back_beambreak_triggered,
- const bool intake_clear_of_box, bool suicide,
- bool trajectory_override, double *proximal_output,
- double *distal_output, bool *release_arm_brake,
- bool *claw_closed, control_loops::ArmStatus *status) {
+flatbuffers::Offset<superstructure::ArmStatus> Arm::Iterate(
+ const ::aos::monotonic_clock::time_point monotonic_now,
+ const uint32_t *unsafe_goal, bool grab_box, bool open_claw, bool close_claw,
+ const superstructure::ArmPosition *position,
+ const bool claw_beambreak_triggered,
+ const bool box_back_beambreak_triggered, const bool intake_clear_of_box,
+ bool suicide, bool trajectory_override, double *proximal_output,
+ double *distal_output, bool *release_arm_brake, bool *claw_closed,
+ flatbuffers::FlatBufferBuilder *fbb) {
::Eigen::Matrix<double, 2, 1> Y;
const bool outputs_disabled =
((proximal_output == nullptr) || (distal_output == nullptr) ||
@@ -86,11 +85,11 @@
claw_closed_count_ = 50;
}
- Y << position->proximal.encoder + proximal_offset_,
- position->distal.encoder + distal_offset_;
+ Y << position->proximal()->encoder() + proximal_offset_,
+ position->distal()->encoder() + distal_offset_;
- proximal_zeroing_estimator_.UpdateEstimate(position->proximal);
- distal_zeroing_estimator_.UpdateEstimate(position->distal);
+ proximal_zeroing_estimator_.UpdateEstimate(*position->proximal());
+ distal_zeroing_estimator_.UpdateEstimate(*position->distal());
if (proximal_output != nullptr) {
*proximal_output = 0.0;
@@ -128,8 +127,8 @@
proximal_offset_ = proximal_zeroing_estimator_.offset();
distal_offset_ = distal_zeroing_estimator_.offset();
- Y << position->proximal.encoder + proximal_offset_,
- position->distal.encoder + distal_offset_;
+ Y << position->proximal()->encoder() + proximal_offset_,
+ position->distal()->encoder() + distal_offset_;
// TODO(austin): Offset ekf rather than reset it. Since we aren't
// moving at this point, it's pretty safe to do this.
@@ -377,17 +376,29 @@
follower_.Update(arm_ekf_.X_hat(), disable, kDt(), vmax_,
max_operating_voltage);
AOS_LOG(INFO, "Max voltage: %f\n", max_operating_voltage);
- status->goal_theta0 = follower_.theta(0);
- status->goal_theta1 = follower_.theta(1);
- status->goal_omega0 = follower_.omega(0);
- status->goal_omega1 = follower_.omega(1);
- status->theta0 = arm_ekf_.X_hat(0);
- status->theta1 = arm_ekf_.X_hat(2);
- status->omega0 = arm_ekf_.X_hat(1);
- status->omega1 = arm_ekf_.X_hat(3);
- status->voltage_error0 = arm_ekf_.X_hat(4);
- status->voltage_error1 = arm_ekf_.X_hat(5);
+ flatbuffers::Offset<frc971::PotAndAbsoluteEncoderEstimatorState>
+ proximal_estimator_state_offset =
+ proximal_zeroing_estimator_.GetEstimatorState(fbb);
+ flatbuffers::Offset<frc971::PotAndAbsoluteEncoderEstimatorState>
+ distal_estimator_state_offset =
+ distal_zeroing_estimator_.GetEstimatorState(fbb);
+
+ superstructure::ArmStatus::Builder status_builder(*fbb);
+ status_builder.add_proximal_estimator_state(proximal_estimator_state_offset);
+ status_builder.add_distal_estimator_state(distal_estimator_state_offset);
+
+ status_builder.add_goal_theta0(follower_.theta(0));
+ status_builder.add_goal_theta1(follower_.theta(1));
+ status_builder.add_goal_omega0(follower_.omega(0));
+ status_builder.add_goal_omega1(follower_.omega(1));
+
+ status_builder.add_theta0(arm_ekf_.X_hat(0));
+ status_builder.add_theta1(arm_ekf_.X_hat(2));
+ status_builder.add_omega0(arm_ekf_.X_hat(1));
+ status_builder.add_omega1(arm_ekf_.X_hat(3));
+ status_builder.add_voltage_error0(arm_ekf_.X_hat(4));
+ status_builder.add_voltage_error1(arm_ekf_.X_hat(5));
if (!disable) {
*proximal_output = ::std::max(
@@ -402,22 +413,17 @@
*claw_closed = claw_closed_;
}
- status->proximal_estimator_state =
- proximal_zeroing_estimator_.GetEstimatorState();
- status->distal_estimator_state =
- distal_zeroing_estimator_.GetEstimatorState();
+ status_builder.add_path_distance_to_go(follower_.path_distance_to_go());
+ status_builder.add_current_node(current_node_);
- status->path_distance_to_go = follower_.path_distance_to_go();
- status->current_node = current_node_;
-
- status->zeroed = (proximal_zeroing_estimator_.zeroed() &&
- distal_zeroing_estimator_.zeroed());
- status->estopped = (state_ == State::ESTOP);
- status->state = static_cast<int32_t>(state_);
- status->grab_state = static_cast<int32_t>(grab_state_);
- status->failed_solutions = follower_.failed_solutions();
+ status_builder.add_zeroed(zeroed());
+ status_builder.add_estopped(estopped());
+ status_builder.add_state(static_cast<int32_t>(state_));
+ status_builder.add_grab_state(static_cast<int32_t>(grab_state_));
+ status_builder.add_failed_solutions(follower_.failed_solutions());
arm_ekf_.Predict(follower_.U(), kDt());
+ return status_builder.Finish();
}
} // namespace arm
diff --git a/y2018/control_loops/superstructure/arm/arm.h b/y2018/control_loops/superstructure/arm/arm.h
index 8cd6b39..7ceb001 100644
--- a/y2018/control_loops/superstructure/arm/arm.h
+++ b/y2018/control_loops/superstructure/arm/arm.h
@@ -1,6 +1,7 @@
#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_ARM_H_
#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_ARM_H_
+#include "aos/time/time.h"
#include "frc971/zeroing/zeroing.h"
#include "y2018/constants.h"
#include "y2018/control_loops/superstructure/arm/dynamics.h"
@@ -8,7 +9,8 @@
#include "y2018/control_loops/superstructure/arm/generated_graph.h"
#include "y2018/control_loops/superstructure/arm/graph.h"
#include "y2018/control_loops/superstructure/arm/trajectory.h"
-#include "y2018/control_loops/superstructure/superstructure.q.h"
+#include "y2018/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2018/control_loops/superstructure/superstructure_status_generated.h"
namespace y2018 {
namespace control_loops {
@@ -34,15 +36,15 @@
static constexpr double kPathlessVMax() { return 5.0; }
static constexpr double kGotoPathVMax() { return 6.0; }
- void Iterate(const ::aos::monotonic_clock::time_point monotonic_now,
- const uint32_t *unsafe_goal, bool grab_box, bool open_claw,
- bool close_claw, const control_loops::ArmPosition *position,
- const bool claw_beambreak_triggered,
- const bool box_back_beambreak_triggered,
- const bool intake_clear_of_box, bool suicide,
- bool trajectory_override, double *proximal_output,
- double *distal_output, bool *release_arm_brake,
- bool *claw_closed, control_loops::ArmStatus *status);
+ flatbuffers::Offset<superstructure::ArmStatus> Iterate(
+ const ::aos::monotonic_clock::time_point monotonic_now,
+ const uint32_t *unsafe_goal, bool grab_box, bool open_claw,
+ bool close_claw, const superstructure::ArmPosition *position,
+ const bool claw_beambreak_triggered,
+ const bool box_back_beambreak_triggered, const bool intake_clear_of_box,
+ bool suicide, bool trajectory_override, double *proximal_output,
+ double *distal_output, bool *release_arm_brake, bool *claw_closed,
+ flatbuffers::FlatBufferBuilder *fbb);
void Reset();
@@ -68,6 +70,12 @@
State state() const { return state_; }
GrabState grab_state() const { return grab_state_; }
+ bool estopped() const { return state_ == State::ESTOP; }
+ bool zeroed() const {
+ return (proximal_zeroing_estimator_.zeroed() &&
+ distal_zeroing_estimator_.zeroed());
+ }
+
// Returns the maximum position for the intake. This is used to override the
// intake position to release the box when the state machine is lifting.
double max_intake_override() const { return max_intake_override_; }
diff --git a/y2018/control_loops/superstructure/intake/BUILD b/y2018/control_loops/superstructure/intake/BUILD
index ca1289e..dde7c63 100644
--- a/y2018/control_loops/superstructure/intake/BUILD
+++ b/y2018/control_loops/superstructure/intake/BUILD
@@ -41,10 +41,11 @@
":intake_plants",
"//aos:math",
"//aos/controls:control_loop",
- "//aos/logging:queue_logging",
- "//frc971/control_loops:queues",
+ "//frc971/control_loops:control_loops_fbs",
"//frc971/zeroing",
"//y2018:constants",
- "//y2018/control_loops/superstructure:superstructure_queue",
+ "//y2018/control_loops/superstructure:superstructure_output_fbs",
+ "//y2018/control_loops/superstructure:superstructure_position_fbs",
+ "//y2018/control_loops/superstructure:superstructure_status_fbs",
],
)
diff --git a/y2018/control_loops/superstructure/intake/intake.cc b/y2018/control_loops/superstructure/intake/intake.cc
index 6c07a49..dffebbc 100644
--- a/y2018/control_loops/superstructure/intake/intake.cc
+++ b/y2018/control_loops/superstructure/intake/intake.cc
@@ -3,9 +3,7 @@
#include <chrono>
#include "aos/commonmath.h"
-#include "aos/controls/control_loops.q.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
#include "y2018/constants.h"
#include "y2018/control_loops/superstructure/intake/intake_delayed_plant.h"
@@ -80,15 +78,15 @@
loop_->Update(disabled);
}
-void IntakeController::SetStatus(IntakeSideStatus *status,
+void IntakeController::SetStatus(IntakeSideStatus::Builder *status,
const double *unsafe_goal) {
- status->goal_position = goal_angle(unsafe_goal);
- status->goal_velocity = loop_->R(1, 0);
- status->spring_position = loop_->X_hat(0) - loop_->X_hat(2);
- status->spring_velocity = loop_->X_hat(1) - loop_->X_hat(3);
- status->motor_position = loop_->X_hat(2);
- status->motor_velocity = loop_->X_hat(3);
- status->delayed_voltage = loop_->X_hat(4);
+ status->add_goal_position(goal_angle(unsafe_goal));
+ status->add_goal_velocity(loop_->R(1, 0));
+ status->add_spring_position(loop_->X_hat(0) - loop_->X_hat(2));
+ status->add_spring_velocity(loop_->X_hat(1) - loop_->X_hat(3));
+ status->add_motor_position(loop_->X_hat(2));
+ status->add_motor_velocity(loop_->X_hat(3));
+ status->add_delayed_voltage(loop_->X_hat(4));
}
IntakeSide::IntakeSide(
@@ -98,11 +96,12 @@
void IntakeSide::Reset() { state_ = State::UNINITIALIZED; }
-void IntakeSide::Iterate(const double *unsafe_goal,
- const control_loops::IntakeElasticSensors *position,
- control_loops::IntakeVoltage *output,
- control_loops::IntakeSideStatus *status) {
- zeroing_estimator_.UpdateEstimate(position->motor_position);
+flatbuffers::Offset<superstructure::IntakeSideStatus> IntakeSide::Iterate(
+ const double *unsafe_goal,
+ const superstructure::IntakeElasticSensors *position,
+ superstructure::IntakeVoltageT *output,
+ flatbuffers::FlatBufferBuilder *fbb) {
+ zeroing_estimator_.UpdateEstimate(*position->motor_position());
switch (state_) {
case State::UNINITIALIZED:
@@ -132,8 +131,8 @@
state_ = State::UNINITIALIZED;
}
// ESTOP if we hit the hard limits.
- if ((status->motor_position) > controller_.intake_range_.upper ||
- (status->motor_position) < controller_.intake_range_.lower) {
+ if ((controller_.motor_position()) > controller_.intake_range().upper_hard ||
+ (controller_.motor_position()) < controller_.intake_range().lower_hard) {
AOS_LOG(ERROR, "Hit hard limits\n");
state_ = State::ESTOP;
}
@@ -145,8 +144,8 @@
}
const bool disable = (output == nullptr) || state_ != State::RUNNING;
- controller_.set_position(position->spring_angle,
- position->motor_position.encoder);
+ controller_.set_position(position->spring_angle(),
+ position->motor_position()->encoder());
controller_.Update(disable, unsafe_goal);
@@ -154,16 +153,25 @@
output->voltage_elastic = controller_.voltage();
}
+ flatbuffers::Offset<frc971::PotAndAbsoluteEncoderEstimatorState>
+ estimator_state = zeroing_estimator_.GetEstimatorState(fbb);
+
+ superstructure::IntakeSideStatus::Builder status_builder(*fbb);
// Save debug/internal state.
- status->estimator_state = zeroing_estimator_.GetEstimatorState();
- controller_.SetStatus(status, unsafe_goal);
- status->calculated_velocity =
- (status->estimator_state.position - intake_last_position_) /
- controller_.kDt;
- status->zeroed = zeroing_estimator_.zeroed();
- status->estopped = (state_ == State::ESTOP);
- status->state = static_cast<int32_t>(state_);
- intake_last_position_ = status->estimator_state.position;
+ status_builder.add_estimator_state(estimator_state);
+
+ controller_.SetStatus(&status_builder, unsafe_goal);
+ status_builder.add_calculated_velocity(
+ (zeroing_estimator_.offset() + position->motor_position()->encoder() -
+ intake_last_position_) /
+ controller_.kDt);
+ status_builder.add_zeroed(zeroing_estimator_.zeroed());
+ status_builder.add_estopped(estopped());
+ status_builder.add_state ( static_cast<int32_t>(state_));
+ intake_last_position_ =
+ zeroing_estimator_.offset() + position->motor_position()->encoder();
+
+ return status_builder.Finish();
}
} // namespace intake
diff --git a/y2018/control_loops/superstructure/intake/intake.h b/y2018/control_loops/superstructure/intake/intake.h
index c3d9772..7f0ec9f 100644
--- a/y2018/control_loops/superstructure/intake/intake.h
+++ b/y2018/control_loops/superstructure/intake/intake.h
@@ -3,12 +3,13 @@
#include "aos/commonmath.h"
#include "aos/controls/control_loop.h"
-#include "frc971/control_loops/control_loops.q.h"
#include "frc971/zeroing/zeroing.h"
#include "y2018/constants.h"
#include "y2018/control_loops/superstructure/intake/intake_delayed_plant.h"
#include "y2018/control_loops/superstructure/intake/intake_plant.h"
-#include "y2018/control_loops/superstructure/superstructure.q.h"
+#include "y2018/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2018/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2018/control_loops/superstructure/superstructure_status_generated.h"
namespace y2018 {
namespace control_loops {
@@ -23,13 +24,14 @@
void set_position(double spring_angle, double output_position);
// Populates the status structure.
- void SetStatus(control_loops::IntakeSideStatus *status,
+ void SetStatus(superstructure::IntakeSideStatus::Builder *status,
const double *unsafe_goal);
// Returns the control loop calculated voltage.
double voltage() const;
double output_position() const { return loop_->X_hat(0); }
+ double motor_position() const { return loop_->X_hat(2); }
// Executes the control loop for a cycle.
void Update(bool disabled, const double *unsafe_goal);
@@ -40,12 +42,6 @@
// Sets the goal angle from unsafe_goal.
double goal_angle(const double *unsafe_goal);
- // The control loop.
- ::std::unique_ptr<
- StateFeedbackLoop<5, 1, 2, double, StateFeedbackPlant<5, 1, 2>,
- StateFeedbackObserver<5, 1, 2>>>
- loop_;
-
constexpr static double kDt =
::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency);
@@ -53,12 +49,22 @@
// possible otherwise zero.
void UpdateOffset(double offset);
+ const ::frc971::constants::Range intake_range() const {
+ return intake_range_;
+ }
+
+ private:
+ // The control loop.
+ ::std::unique_ptr<
+ StateFeedbackLoop<5, 1, 2, double, StateFeedbackPlant<5, 1, 2>,
+ StateFeedbackObserver<5, 1, 2>>>
+ loop_;
+
const ::frc971::constants::Range intake_range_;
// Stores the current zeroing estimator offset.
double offset_ = 0.0;
- private:
bool reset_ = true;
// The current sensor measurement.
@@ -75,10 +81,11 @@
// The operating voltage.
static constexpr double kOperatingVoltage() { return 12.0; }
- void Iterate(const double *unsafe_goal,
- const control_loops::IntakeElasticSensors *position,
- control_loops::IntakeVoltage *output,
- control_loops::IntakeSideStatus *status);
+ flatbuffers::Offset<superstructure::IntakeSideStatus> Iterate(
+ const double *unsafe_goal,
+ const superstructure::IntakeElasticSensors *position,
+ superstructure::IntakeVoltageT *output,
+ flatbuffers::FlatBufferBuilder *fbb);
void Reset();
@@ -91,6 +98,9 @@
State state() const { return state_; }
+ bool estopped() const { return state_ == State::ESTOP; }
+ bool zeroed() const { return zeroing_estimator_.zeroed(); }
+
bool clear_of_box() const { return controller_.output_position() < -0.1; }
double output_position() const { return controller_.output_position(); }
diff --git a/y2018/control_loops/superstructure/superstructure.cc b/y2018/control_loops/superstructure/superstructure.cc
index a2dcaf6..70af274 100644
--- a/y2018/control_loops/superstructure/superstructure.cc
+++ b/y2018/control_loops/superstructure/superstructure.cc
@@ -2,14 +2,13 @@
#include <chrono>
-#include "aos/controls/control_loops.q.h"
#include "aos/logging/logging.h"
-#include "frc971/control_loops/control_loops.q.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
#include "y2018/constants.h"
#include "y2018/control_loops/superstructure/intake/intake.h"
-#include "y2018/status_light.q.h"
-#include "y2018/vision/vision.q.h"
+#include "y2018/status_light_generated.h"
+#include "y2018/vision/vision_generated.h"
namespace y2018 {
namespace control_loops {
@@ -26,25 +25,23 @@
Superstructure::Superstructure(::aos::EventLoop *event_loop,
const ::std::string &name)
- : aos::controls::ControlLoop<control_loops::SuperstructureQueue>(event_loop,
- name),
+ : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+ name),
status_light_sender_(
- event_loop->MakeSender<::y2018::StatusLight>(".y2018.status_light")),
+ event_loop->MakeSender<::y2018::StatusLight>("/superstructure")),
vision_status_fetcher_(
event_loop->MakeFetcher<::y2018::vision::VisionStatus>(
- ".y2018.vision.vision_status")),
+ "/superstructure")),
drivetrain_output_fetcher_(
- event_loop
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Output>(
- ".frc971.control_loops.drivetrain_queue.output")),
+ event_loop->MakeFetcher<::frc971::control_loops::drivetrain::Output>(
+ "/drivetrain")),
intake_left_(constants::GetValues().left_intake.zeroing),
intake_right_(constants::GetValues().right_intake.zeroing) {}
-void Superstructure::RunIteration(
- const control_loops::SuperstructureQueue::Goal *unsafe_goal,
- const control_loops::SuperstructureQueue::Position *position,
- control_loops::SuperstructureQueue::Output *output,
- control_loops::SuperstructureQueue::Status *status) {
+void Superstructure::RunIteration(const Goal *unsafe_goal,
+ const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) {
const monotonic_clock::time_point monotonic_now =
event_loop()->monotonic_now();
if (WasReset()) {
@@ -55,7 +52,7 @@
}
const double clipped_box_distance =
- ::std::min(1.0, ::std::max(0.0, position->box_distance));
+ ::std::min(1.0, ::std::max(0.0, position->box_distance()));
const double box_velocity =
(clipped_box_distance - last_box_distance_) / 0.005;
@@ -64,31 +61,34 @@
filtered_box_velocity_ =
box_velocity * kFilteredBoxVelocityAlpha +
(1.0 - kFilteredBoxVelocityAlpha) * filtered_box_velocity_;
- status->filtered_box_velocity = filtered_box_velocity_;
constexpr double kCenteringAngleGain = 0.0;
const double left_intake_goal =
- ::std::min(
- arm_.max_intake_override(),
- (unsafe_goal == nullptr ? 0.0
- : unsafe_goal->intake.left_intake_angle)) +
+ ::std::min(arm_.max_intake_override(),
+ (unsafe_goal == nullptr || !unsafe_goal->has_intake()
+ ? 0.0
+ : unsafe_goal->intake()->left_intake_angle())) +
last_intake_center_error_ * kCenteringAngleGain;
const double right_intake_goal =
- ::std::min(
- arm_.max_intake_override(),
- (unsafe_goal == nullptr ? 0.0
- : unsafe_goal->intake.right_intake_angle)) -
+ ::std::min(arm_.max_intake_override(),
+ (unsafe_goal == nullptr || !unsafe_goal->has_intake()
+ ? 0.0
+ : unsafe_goal->intake()->right_intake_angle())) -
last_intake_center_error_ * kCenteringAngleGain;
- intake_left_.Iterate(unsafe_goal != nullptr ? &(left_intake_goal) : nullptr,
- &(position->left_intake),
- output != nullptr ? &(output->left_intake) : nullptr,
- &(status->left_intake));
+ IntakeVoltageT left_intake_output;
+ flatbuffers::Offset<superstructure::IntakeSideStatus> left_status_offset =
+ intake_left_.Iterate(
+ unsafe_goal != nullptr ? &(left_intake_goal) : nullptr,
+ position->left_intake(),
+ output != nullptr ? &(left_intake_output) : nullptr, status->fbb());
- intake_right_.Iterate(unsafe_goal != nullptr ? &(right_intake_goal) : nullptr,
- &(position->right_intake),
- output != nullptr ? &(output->right_intake) : nullptr,
- &(status->right_intake));
+ IntakeVoltageT right_intake_output;
+ flatbuffers::Offset<superstructure::IntakeSideStatus> right_status_offset =
+ intake_right_.Iterate(
+ unsafe_goal != nullptr ? &(right_intake_goal) : nullptr,
+ position->right_intake(),
+ output != nullptr ? &(right_intake_output) : nullptr, status->fbb());
const double intake_center_error =
intake_right_.output_position() - intake_left_.output_position();
@@ -97,63 +97,81 @@
const bool intake_clear_of_box =
intake_left_.clear_of_box() && intake_right_.clear_of_box();
- bool open_claw = unsafe_goal != nullptr ? unsafe_goal->open_claw : false;
+ bool open_claw = unsafe_goal != nullptr ? unsafe_goal->open_claw() : false;
if (unsafe_goal) {
- if (unsafe_goal->open_threshold != 0.0) {
- if (arm_.current_node() != unsafe_goal->arm_goal_position ||
- arm_.path_distance_to_go() > unsafe_goal->open_threshold) {
+ if (unsafe_goal->open_threshold() != 0.0) {
+ if (arm_.current_node() != unsafe_goal->arm_goal_position() ||
+ arm_.path_distance_to_go() > unsafe_goal->open_threshold()) {
open_claw = false;
}
}
}
- arm_.Iterate(
- monotonic_now,
- unsafe_goal != nullptr ? &(unsafe_goal->arm_goal_position) : nullptr,
- unsafe_goal != nullptr ? unsafe_goal->grab_box : false, open_claw,
- unsafe_goal != nullptr ? unsafe_goal->close_claw : false,
- &(position->arm), position->claw_beambreak_triggered,
- position->box_back_beambreak_triggered, intake_clear_of_box,
- unsafe_goal != nullptr ? unsafe_goal->voltage_winch > 1.0 : false,
- unsafe_goal != nullptr ? unsafe_goal->trajectory_override : false,
- output != nullptr ? &(output->voltage_proximal) : nullptr,
- output != nullptr ? &(output->voltage_distal) : nullptr,
- output != nullptr ? &(output->release_arm_brake) : nullptr,
- output != nullptr ? &(output->claw_grabbed) : nullptr, &(status->arm));
+ const uint32_t arm_goal_position =
+ unsafe_goal != nullptr ? unsafe_goal->arm_goal_position() : 0u;
+
+ double voltage_proximal_output = 0.0;
+ double voltage_distal_output = 0.0;
+ bool release_arm_brake_output = false;
+ bool claw_grabbed_output = false;
+ flatbuffers::Offset<superstructure::ArmStatus> arm_status_offset =
+ arm_.Iterate(
+ monotonic_now,
+ unsafe_goal != nullptr ? &(arm_goal_position) : nullptr,
+ unsafe_goal != nullptr ? unsafe_goal->grab_box() : false, open_claw,
+ unsafe_goal != nullptr ? unsafe_goal->close_claw() : false,
+ position->arm(), position->claw_beambreak_triggered(),
+ position->box_back_beambreak_triggered(), intake_clear_of_box,
+ unsafe_goal != nullptr ? unsafe_goal->voltage_winch() > 1.0 : false,
+ unsafe_goal != nullptr ? unsafe_goal->trajectory_override() : false,
+ output != nullptr ? &voltage_proximal_output : nullptr,
+ output != nullptr ? &voltage_distal_output : nullptr,
+ output != nullptr ? &release_arm_brake_output : nullptr,
+ output != nullptr ? &claw_grabbed_output : nullptr, status->fbb());
+
+
+ bool hook_release_output = false;
+ bool forks_release_output = false;
+ double voltage_winch_output = 0.0;
if (output) {
if (unsafe_goal) {
- output->hook_release = unsafe_goal->hook_release;
- output->voltage_winch = unsafe_goal->voltage_winch;
- output->forks_release = unsafe_goal->deploy_fork;
- } else {
- output->voltage_winch = 0.0;
- output->hook_release = false;
- output->forks_release = false;
+ hook_release_output = unsafe_goal->hook_release();
+ voltage_winch_output = unsafe_goal->voltage_winch();
+ forks_release_output = unsafe_goal->deploy_fork();
}
}
- status->estopped = status->left_intake.estopped ||
- status->right_intake.estopped || status->arm.estopped;
+ Status::Builder status_builder = status->MakeBuilder<Status>();
- status->zeroed = status->left_intake.zeroed && status->right_intake.zeroed &&
- status->arm.zeroed;
+ status_builder.add_left_intake(left_status_offset);
+ status_builder.add_right_intake(right_status_offset);
+ status_builder.add_arm(arm_status_offset);
+
+ status_builder.add_filtered_box_velocity(filtered_box_velocity_);
+ const bool estopped =
+ intake_left_.estopped() || intake_right_.estopped() || arm_.estopped();
+ status_builder.add_estopped(estopped);
+
+ status_builder.add_zeroed(intake_left_.zeroed() && intake_right_.zeroed() &&
+ arm_.zeroed());
if (output && unsafe_goal) {
- double roller_voltage = ::std::max(
- -kMaxIntakeRollerVoltage, ::std::min(unsafe_goal->intake.roller_voltage,
- kMaxIntakeRollerVoltage));
+ double roller_voltage =
+ ::std::max(-kMaxIntakeRollerVoltage,
+ ::std::min(unsafe_goal->intake()->roller_voltage(),
+ kMaxIntakeRollerVoltage));
constexpr int kReverseTime = 14;
- if (unsafe_goal->intake.roller_voltage < 0.0 ||
- unsafe_goal->disable_box_correct) {
- output->left_intake.voltage_rollers = roller_voltage;
- output->right_intake.voltage_rollers = roller_voltage;
+ if (unsafe_goal->intake()->roller_voltage() < 0.0 ||
+ unsafe_goal->disable_box_correct()) {
+ left_intake_output.voltage_rollers = roller_voltage;
+ right_intake_output.voltage_rollers = roller_voltage;
rotation_state_ = RotationState::NOT_ROTATING;
rotation_count_ = 0;
stuck_count_ = 0;
} else {
- const bool stuck = position->box_distance < 0.20 &&
- filtered_box_velocity_ > -0.05 &&
- !position->box_back_beambreak_triggered;
+ const bool stuck = position->box_distance() < 0.20 &&
+ filtered_box_velocity_ > -0.05 &&
+ !position->box_back_beambreak_triggered();
// Make sure we don't declare ourselves re-stuck too quickly. We want to
// wait 400 ms before triggering the stuck condition again.
if (!stuck) {
@@ -171,11 +189,11 @@
rotation_state_ = RotationState::STUCK;
++stuck_count_;
last_stuck_time_ = monotonic_now;
- } else if (position->left_intake.beam_break) {
+ } else if (position->left_intake()->beam_break()) {
rotation_state_ = RotationState::ROTATING_RIGHT;
rotation_count_ = kReverseTime;
break;
- } else if (position->right_intake.beam_break) {
+ } else if (position->right_intake()->beam_break()) {
rotation_state_ = RotationState::ROTATING_LEFT;
rotation_count_ = kReverseTime;
break;
@@ -190,7 +208,7 @@
}
} break;
case RotationState::ROTATING_LEFT:
- if (position->right_intake.beam_break) {
+ if (position->right_intake()->beam_break()) {
rotation_count_ = kReverseTime;
} else {
--rotation_count_;
@@ -200,7 +218,7 @@
}
break;
case RotationState::ROTATING_RIGHT:
- if (position->left_intake.beam_break) {
+ if (position->left_intake()->beam_break()) {
rotation_count_ = kReverseTime;
} else {
--rotation_count_;
@@ -214,7 +232,7 @@
constexpr double kHoldVoltage = 1.0;
constexpr double kStuckVoltage = 10.0;
- if (position->box_back_beambreak_triggered &&
+ if (position->box_back_beambreak_triggered() &&
roller_voltage > kHoldVoltage) {
roller_voltage = kHoldVoltage;
}
@@ -226,31 +244,31 @@
centering_gain = 0.0;
}
}
- output->left_intake.voltage_rollers =
+ left_intake_output.voltage_rollers =
roller_voltage - intake_center_error * centering_gain;
- output->right_intake.voltage_rollers =
+ right_intake_output.voltage_rollers =
roller_voltage + intake_center_error * centering_gain;
} break;
case RotationState::STUCK: {
if (roller_voltage > kHoldVoltage) {
- output->left_intake.voltage_rollers = -kStuckVoltage;
- output->right_intake.voltage_rollers = -kStuckVoltage;
+ left_intake_output.voltage_rollers = -kStuckVoltage;
+ right_intake_output.voltage_rollers = -kStuckVoltage;
}
} break;
case RotationState::ROTATING_LEFT:
- if (position->left_intake.beam_break) {
- output->left_intake.voltage_rollers = -roller_voltage * 0.9;
+ if (position->left_intake()->beam_break()) {
+ left_intake_output.voltage_rollers = -roller_voltage * 0.9;
} else {
- output->left_intake.voltage_rollers = -roller_voltage * 0.6;
+ left_intake_output.voltage_rollers = -roller_voltage * 0.6;
}
- output->right_intake.voltage_rollers = roller_voltage;
+ right_intake_output.voltage_rollers = roller_voltage;
break;
case RotationState::ROTATING_RIGHT:
- output->left_intake.voltage_rollers = roller_voltage;
- if (position->right_intake.beam_break) {
- output->right_intake.voltage_rollers = -roller_voltage * 0.9;
+ left_intake_output.voltage_rollers = roller_voltage;
+ if (position->right_intake()->beam_break()) {
+ right_intake_output.voltage_rollers = -roller_voltage * 0.9;
} else {
- output->right_intake.voltage_rollers = -roller_voltage * 0.6;
+ right_intake_output.voltage_rollers = -roller_voltage * 0.6;
}
break;
}
@@ -260,29 +278,31 @@
rotation_count_ = 0;
stuck_count_ = 0;
}
- status->rotation_state = static_cast<uint32_t>(rotation_state_);
+ status_builder.add_rotation_state(static_cast<uint32_t>(rotation_state_));
drivetrain_output_fetcher_.Fetch();
vision_status_fetcher_.Fetch();
- if (status->estopped) {
+ if (estopped) {
SendColors(0.5, 0.0, 0.0);
} else if (!vision_status_fetcher_.get() ||
monotonic_now >
- vision_status_fetcher_->sent_time + chrono::seconds(1)) {
+ vision_status_fetcher_.context().monotonic_sent_time +
+ chrono::seconds(1)) {
SendColors(0.5, 0.5, 0.0);
} else if (rotation_state_ == RotationState::ROTATING_LEFT ||
rotation_state_ == RotationState::ROTATING_RIGHT) {
SendColors(0.5, 0.20, 0.0);
} else if (rotation_state_ == RotationState::STUCK) {
SendColors(0.5, 0.0, 0.5);
- } else if (position->box_back_beambreak_triggered) {
+ } else if (position->box_back_beambreak_triggered()) {
SendColors(0.0, 0.0, 0.5);
- } else if (position->box_distance < 0.2) {
+ } else if (position->box_distance() < 0.2) {
SendColors(0.0, 0.5, 0.0);
} else if (drivetrain_output_fetcher_.get() &&
- ::std::max(::std::abs(drivetrain_output_fetcher_->left_voltage),
- ::std::abs(drivetrain_output_fetcher_->right_voltage)) >
+ ::std::max(
+ ::std::abs(drivetrain_output_fetcher_->left_voltage()),
+ ::std::abs(drivetrain_output_fetcher_->right_voltage())) >
11.5) {
SendColors(0.5, 0.0, 0.5);
} else {
@@ -290,15 +310,41 @@
}
last_box_distance_ = clipped_box_distance;
+
+ if (output) {
+ flatbuffers::Offset<IntakeVoltage> left_intake_offset =
+ IntakeVoltage::Pack(*output->fbb(), &left_intake_output);
+ flatbuffers::Offset<IntakeVoltage> right_intake_offset =
+ IntakeVoltage::Pack(*output->fbb(), &right_intake_output);
+
+ Output::Builder output_builder = output->MakeBuilder<Output>();
+ output_builder.add_left_intake(left_intake_offset);
+ output_builder.add_right_intake(right_intake_offset);
+ output_builder.add_voltage_proximal(voltage_proximal_output);
+ output_builder.add_voltage_distal(voltage_distal_output);
+ output_builder.add_release_arm_brake(release_arm_brake_output);
+ output_builder.add_claw_grabbed(claw_grabbed_output);
+
+ output_builder.add_hook_release(hook_release_output);
+ output_builder.add_forks_release(forks_release_output);
+ output_builder.add_voltage_winch(voltage_winch_output);
+
+ output->Send(output_builder.Finish());
+ }
+
+ status->Send(status_builder.Finish());
}
void Superstructure::SendColors(float red, float green, float blue) {
- auto new_status_light = status_light_sender_.MakeMessage();
- new_status_light->red = red;
- new_status_light->green = green;
- new_status_light->blue = blue;
+ auto builder = status_light_sender_.MakeBuilder();
+ StatusLight::Builder status_light_builder =
+ builder.MakeBuilder<StatusLight>();
- if (!new_status_light.Send()) {
+ status_light_builder.add_red(red);
+ status_light_builder.add_green(green);
+ status_light_builder.add_blue(blue);
+
+ if (!builder.Send(status_light_builder.Finish())) {
AOS_LOG(ERROR, "Failed to send lights.\n");
}
}
diff --git a/y2018/control_loops/superstructure/superstructure.h b/y2018/control_loops/superstructure/superstructure.h
index 81b6d9d..78851df 100644
--- a/y2018/control_loops/superstructure/superstructure.h
+++ b/y2018/control_loops/superstructure/superstructure.h
@@ -4,36 +4,37 @@
#include <memory>
#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 "aos/time/time.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "y2018/control_loops/superstructure/arm/arm.h"
#include "y2018/control_loops/superstructure/intake/intake.h"
-#include "y2018/control_loops/superstructure/superstructure.q.h"
-#include "y2018/status_light.q.h"
-#include "y2018/vision/vision.q.h"
+#include "y2018/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2018/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2018/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2018/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2018/status_light_generated.h"
+#include "y2018/vision/vision_generated.h"
namespace y2018 {
namespace control_loops {
namespace superstructure {
class Superstructure
- : public ::aos::controls::ControlLoop<control_loops::SuperstructureQueue> {
+ : public ::aos::controls::ControlLoop<Goal, Position, Status, Output> {
public:
- explicit Superstructure(
- ::aos::EventLoop *event_loop,
- const ::std::string &name = ".y2018.control_loops.superstructure_queue");
+ explicit Superstructure(::aos::EventLoop *event_loop,
+ const ::std::string &name = "/superstructure");
const intake::IntakeSide &intake_left() const { return intake_left_; }
const intake::IntakeSide &intake_right() const { return intake_right_; }
const arm::Arm &arm() const { return arm_; }
protected:
- virtual void RunIteration(
- const control_loops::SuperstructureQueue::Goal *unsafe_goal,
- const control_loops::SuperstructureQueue::Position *position,
- control_loops::SuperstructureQueue::Output *output,
- control_loops::SuperstructureQueue::Status *status) override;
+ virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) override;
private:
// Sends the status light message for the 3 colors provided.
@@ -41,7 +42,7 @@
::aos::Sender<::y2018::StatusLight> status_light_sender_;
::aos::Fetcher<::y2018::vision::VisionStatus> vision_status_fetcher_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Output>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Output>
drivetrain_output_fetcher_;
intake::IntakeSide intake_left_;
diff --git a/y2018/control_loops/superstructure/superstructure.q b/y2018/control_loops/superstructure/superstructure.q
deleted file mode 100644
index 049a406..0000000
--- a/y2018/control_loops/superstructure/superstructure.q
+++ /dev/null
@@ -1,222 +0,0 @@
-package y2018.control_loops;
-
-import "aos/controls/control_loops.q";
-import "frc971/control_loops/control_loops.q";
-
-struct IntakeSideStatus {
- // Is the subsystem zeroed?
- bool zeroed;
-
- // The state of the subsystem, if applicable.
- int32_t state;
-
- // If true, we have aborted.
- bool estopped;
-
- // Estimated position of the spring.
- float spring_position;
- // Estimated velocity of the spring in units/second.
- float spring_velocity;
-
- // Estimated position of the joint.
- float motor_position;
- // Estimated velocity of the joint in units/second.
- float motor_velocity;
-
- // Goal position of the joint.
- float goal_position;
- // Goal velocity of the joint in units/second.
- float goal_velocity;
-
- // The calculated velocity with delta x/delta t
- float calculated_velocity;
-
- // The voltage given last cycle;
- float delayed_voltage;
-
- // State of the estimator.
- .frc971.PotAndAbsoluteEncoderEstimatorState estimator_state;
-};
-
-struct IntakeGoal {
- double roller_voltage;
-
- // Goal angle in radians of the intake.
- // Zero radians is where the intake is pointing straight out, with positive
- // radians inward towards the cube.
- double left_intake_angle;
- double right_intake_angle;
-};
-
-struct IntakeElasticSensors {
- // Position of the motor end of the series elastic in radians.
- .frc971.PotAndAbsolutePosition motor_position;
-
- // Displacement of the spring in radians.
- double spring_angle;
-
- // False if the beam break sensor isn't triggered, true if the beam breaker is
- // triggered.
- bool beam_break;
-};
-
-struct ArmStatus {
- // State of the estimators.
- .frc971.PotAndAbsoluteEncoderEstimatorState proximal_estimator_state;
- .frc971.PotAndAbsoluteEncoderEstimatorState distal_estimator_state;
-
- // The node we are currently going to.
- uint32_t current_node;
- // Distance (in radians) to the end of the path.
- float path_distance_to_go;
- // Goal position and velocity (radians)
- float goal_theta0;
- float goal_theta1;
- float goal_omega0;
- float goal_omega1;
-
- // Current position and velocity (radians)
- float theta0;
- float theta1;
-
- float omega0;
- float omega1;
-
- // Estimated voltage error for the two joints.
- float voltage_error0;
- float voltage_error1;
-
- // True if we are zeroed.
- bool zeroed;
-
- // True if the arm is zeroed.
- bool estopped;
-
- // The current state machine state.
- uint32_t state;
-
- uint32_t grab_state;
-
- // The number of times the LQR solver failed.
- uint32_t failed_solutions;
-};
-
-struct ArmPosition {
- // Values of the encoder and potentiometer at the base of the proximal
- // (connected to drivebase) arm in radians.
- .frc971.PotAndAbsolutePosition proximal;
-
- // Values of the encoder and potentiometer at the base of the distal
- // (connected to proximal) arm in radians.
- .frc971.PotAndAbsolutePosition distal;
-};
-
-struct IntakeVoltage {
- // Voltage of the motors on the series elastic on one side (left or right) of
- // the intake.
- double voltage_elastic;
-
- // Voltage of the rollers on one side (left or right) of the intake.
- double voltage_rollers;
-};
-
-// Published on ".y2018.control_loops.superstructure_queue"
-queue_group SuperstructureQueue {
- implements aos.control_loops.ControlLoop;
-
- message Goal {
- IntakeGoal intake;
-
- // Used to identiy a position in the planned set of positions on the arm.
- uint32_t arm_goal_position;
- // If true, start the grab box sequence.
- bool grab_box;
-
- bool open_claw;
- bool close_claw;
-
- bool deploy_fork;
-
- bool hook_release;
-
- double voltage_winch;
-
- double open_threshold;
-
- bool disable_box_correct;
-
- bool trajectory_override;
- };
-
- message Status {
- // Are all the subsystems zeroed?
- bool zeroed;
-
- // If true, any of the subsystems have aborted.
- bool estopped;
-
- // Status of both intake sides.
- IntakeSideStatus left_intake;
- IntakeSideStatus right_intake;
-
- ArmStatus arm;
-
- double filtered_box_velocity;
- uint32_t rotation_state;
- };
-
- message Position {
- // Values of the series elastic encoders on the left side of the robot from
- // the rear perspective in radians.
- IntakeElasticSensors left_intake;
-
- // Values of the series elastic encoders on the right side of the robot from
- // the rear perspective in radians.
- IntakeElasticSensors right_intake;
-
- ArmPosition arm;
-
- // Value of the beam breaker sensor. This value is true if the beam is
- // broken, false if the beam isn't broken.
- bool claw_beambreak_triggered;
- // Value of the beambreak sensor detecting when the box has hit the frame
- // cutout.
- bool box_back_beambreak_triggered;
-
- // Distance to the box in meters.
- double box_distance;
- };
-
- message Output {
- // Voltage sent to the parts on the left side of the intake.
- IntakeVoltage left_intake;
-
- // Voltage sent to the parts on the right side of the intake.
- IntakeVoltage right_intake;
-
- // Voltage sent to the motors on the proximal joint of the arm.
- double voltage_proximal;
-
- // Voltage sent to the motors on the distal joint of the arm.
- double voltage_distal;
-
- // Voltage sent to the hanger. Positive pulls the robot up.
- double voltage_winch;
-
- // Clamped (when true) or unclamped (when false) status sent to the
- // pneumatic claw on the arm.
- bool claw_grabbed;
-
- // If true, release the arm brakes.
- bool release_arm_brake;
- // If true, release the hook
- bool hook_release;
- // If true, release the forks
- bool forks_release;
- };
-
- queue Goal goal;
- queue Output output;
- queue Status status;
- queue Position position;
-};
diff --git a/y2018/control_loops/superstructure/superstructure_goal.fbs b/y2018/control_loops/superstructure/superstructure_goal.fbs
new file mode 100644
index 0000000..b618c1b
--- /dev/null
+++ b/y2018/control_loops/superstructure/superstructure_goal.fbs
@@ -0,0 +1,39 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2018.control_loops.superstructure;
+
+table IntakeGoal {
+ roller_voltage:double;
+
+ // Goal angle in radians of the intake.
+ // Zero radians is where the intake is pointing straight out, with positive
+ // radians inward towards the cube.
+ left_intake_angle:double;
+ right_intake_angle:double;
+}
+
+table Goal {
+ intake:IntakeGoal;
+
+ // Used to identiy a position in the planned set of positions on the arm.
+ arm_goal_position:uint;
+ // If true, start the grab box sequence.
+ grab_box:bool;
+
+ open_claw:bool;
+ close_claw:bool;
+
+ deploy_fork:bool;
+
+ hook_release:bool;
+
+ voltage_winch:double;
+
+ open_threshold:double;
+
+ disable_box_correct:bool;
+
+ trajectory_override:bool;
+}
+
+root_type Goal;
diff --git a/y2018/control_loops/superstructure/superstructure_lib_test.cc b/y2018/control_loops/superstructure/superstructure_lib_test.cc
index 162db33..988655f 100644
--- a/y2018/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2018/control_loops/superstructure/superstructure_lib_test.cc
@@ -6,8 +6,6 @@
#include <memory>
#include "aos/controls/control_loop_test.h"
-#include "aos/queue.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
@@ -15,8 +13,8 @@
#include "y2018/control_loops/superstructure/arm/dynamics.h"
#include "y2018/control_loops/superstructure/arm/generated_graph.h"
#include "y2018/control_loops/superstructure/intake/intake_plant.h"
-#include "y2018/status_light.q.h"
-#include "y2018/vision/vision.q.h"
+#include "y2018/status_light_generated.h"
+#include "y2018/vision/vision_generated.h"
using ::frc971::control_loops::PositionSensorSimulator;
@@ -71,9 +69,18 @@
zeroing_constants_.measured_absolute_position);
}
- void GetSensorValues(IntakeElasticSensors *position) {
- pot_encoder_.GetSensorValues(&position->motor_position);
- position->spring_angle = plant_.Y(0);
+ flatbuffers::Offset<IntakeElasticSensors> GetSensorValues(
+ flatbuffers::FlatBufferBuilder *fbb) {
+ frc971::PotAndAbsolutePosition::Builder motor_position_builder(*fbb);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> motor_position_offset =
+ pot_encoder_.GetSensorValues(&motor_position_builder);
+
+ IntakeElasticSensors::Builder intake_elastic_sensors_builder(*fbb);
+
+ intake_elastic_sensors_builder.add_motor_position(motor_position_offset);
+ intake_elastic_sensors_builder.add_spring_angle(plant_.Y(0));
+
+ return intake_elastic_sensors_builder.Finish();
}
double spring_position() const { return plant_.X(0); }
@@ -85,14 +92,14 @@
plant_.set_voltage_offset(voltage_offset);
}
- void Simulate(const IntakeVoltage &intake_voltage) {
+ void Simulate(const IntakeVoltage *intake_voltage) {
const double voltage_check =
superstructure::intake::IntakeSide::kOperatingVoltage();
- AOS_CHECK_LE(::std::abs(intake_voltage.voltage_elastic), voltage_check);
+ AOS_CHECK_LE(::std::abs(intake_voltage->voltage_elastic()), voltage_check);
::Eigen::Matrix<double, 1, 1> U;
- U << intake_voltage.voltage_elastic + plant_.voltage_offset();
+ U << intake_voltage->voltage_elastic() + plant_.voltage_offset();
plant_.Update(U);
@@ -141,9 +148,21 @@
distal_zeroing_constants_.measured_absolute_position);
}
- void GetSensorValues(ArmPosition *position) {
- proximal_pot_encoder_.GetSensorValues(&position->proximal);
- distal_pot_encoder_.GetSensorValues(&position->distal);
+ flatbuffers::Offset<ArmPosition> GetSensorValues(
+ flatbuffers::FlatBufferBuilder *fbb) {
+ frc971::PotAndAbsolutePosition::Builder proximal_builder(*fbb);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> proximal_offset =
+ proximal_pot_encoder_.GetSensorValues(&proximal_builder);
+
+ frc971::PotAndAbsolutePosition::Builder distal_builder(*fbb);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> distal_offset =
+ distal_pot_encoder_.GetSensorValues(&distal_builder);
+
+ ArmPosition::Builder arm_position_builder(*fbb);
+ arm_position_builder.add_proximal(proximal_offset);
+ arm_position_builder.add_distal(distal_offset);
+
+ return arm_position_builder.Finish();
}
double proximal_position() const { return X_(0, 0); }
@@ -184,7 +203,6 @@
PositionSensorSimulator distal_pot_encoder_;
};
-
class SuperstructureSimulation {
public:
SuperstructureSimulation(::aos::EventLoop *event_loop)
@@ -198,14 +216,14 @@
arm_(constants::GetValues().arm_proximal.zeroing,
constants::GetValues().arm_distal.zeroing),
superstructure_position_sender_(
- event_loop_->MakeSender<SuperstructureQueue::Position>(
- ".y2018.control_loops.superstructure.position")),
+ event_loop_->MakeSender<superstructure::Position>(
+ "/superstructure")),
superstructure_status_fetcher_(
- event_loop_->MakeFetcher<SuperstructureQueue::Status>(
- ".y2018.control_loops.superstructure.status")),
+ event_loop_->MakeFetcher<superstructure::Status>(
+ "/superstructure")),
superstructure_output_fetcher_(
- event_loop_->MakeFetcher<SuperstructureQueue::Output>(
- ".y2018.control_loops.superstructure.output")) {
+ event_loop_->MakeFetcher<superstructure::Output>(
+ "/superstructure")) {
// Start the intake out in the middle by default.
InitializeIntakePosition((constants::Values::kIntakeRange().lower +
constants::Values::kIntakeRange().upper) /
@@ -235,13 +253,20 @@
}
void SendPositionMessage() {
- auto position = superstructure_position_sender_.MakeMessage();
+ auto builder = superstructure_position_sender_.MakeBuilder();
- left_intake_.GetSensorValues(&position->left_intake);
- right_intake_.GetSensorValues(&position->right_intake);
- arm_.GetSensorValues(&position->arm);
- AOS_LOG_STRUCT(INFO, "sim position", *position);
- position.Send();
+ flatbuffers::Offset<IntakeElasticSensors> left_intake_offset =
+ left_intake_.GetSensorValues(builder.fbb());
+ flatbuffers::Offset<IntakeElasticSensors> right_intake_offset =
+ right_intake_.GetSensorValues(builder.fbb());
+ flatbuffers::Offset<ArmPosition> arm_offset =
+ arm_.GetSensorValues(builder.fbb());
+
+ Position::Builder position_builder = builder.MakeBuilder<Position>();
+ position_builder.add_left_intake(left_intake_offset);
+ position_builder.add_right_intake(right_intake_offset);
+ position_builder.add_arm(arm_offset);
+ EXPECT_TRUE(builder.Send(position_builder.Finish()));
}
// Sets the difference between the commanded and applied powers.
@@ -263,13 +288,13 @@
ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
- left_intake_.Simulate(superstructure_output_fetcher_->left_intake);
- right_intake_.Simulate(superstructure_output_fetcher_->right_intake);
+ left_intake_.Simulate(superstructure_output_fetcher_->left_intake());
+ right_intake_.Simulate(superstructure_output_fetcher_->right_intake());
arm_.Simulate((::Eigen::Matrix<double, 2, 1>()
- << superstructure_output_fetcher_->voltage_proximal,
- superstructure_output_fetcher_->voltage_distal)
+ << superstructure_output_fetcher_->voltage_proximal(),
+ superstructure_output_fetcher_->voltage_distal())
.finished(),
- superstructure_output_fetcher_->release_arm_brake);
+ superstructure_output_fetcher_->release_arm_brake());
}
private:
@@ -280,9 +305,9 @@
IntakeSideSimulation right_intake_;
ArmSimulation arm_;
- ::aos::Sender<SuperstructureQueue::Position> superstructure_position_sender_;
- ::aos::Fetcher<SuperstructureQueue::Status> superstructure_status_fetcher_;
- ::aos::Fetcher<SuperstructureQueue::Output> superstructure_output_fetcher_;
+ ::aos::Sender<superstructure::Position> superstructure_position_sender_;
+ ::aos::Fetcher<superstructure::Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<superstructure::Output> superstructure_output_fetcher_;
bool first_ = true;
};
@@ -290,23 +315,24 @@
class SuperstructureTest : public ::aos::testing::ControlLoopTest {
protected:
SuperstructureTest()
- : ::aos::testing::ControlLoopTest(::std::chrono::microseconds(5050)),
+ : ::aos::testing::ControlLoopTest(
+ aos::configuration::ReadConfig("y2018/config.json"),
+ ::std::chrono::microseconds(5050)),
test_event_loop_(MakeEventLoop()),
superstructure_goal_fetcher_(
- test_event_loop_->MakeFetcher<SuperstructureQueue::Goal>(
- ".y2018.control_loops.superstructure.goal")),
+ test_event_loop_->MakeFetcher<superstructure::Goal>(
+ "/superstructure")),
superstructure_goal_sender_(
- test_event_loop_->MakeSender<SuperstructureQueue::Goal>(
- ".y2018.control_loops.superstructure.goal")),
+ test_event_loop_->MakeSender<superstructure::Goal>(
+ "/superstructure")),
superstructure_status_fetcher_(
- test_event_loop_->MakeFetcher<SuperstructureQueue::Status>(
- ".y2018.control_loops.superstructure.status")),
+ test_event_loop_->MakeFetcher<superstructure::Status>(
+ "/superstructure")),
superstructure_output_fetcher_(
- test_event_loop_->MakeFetcher<SuperstructureQueue::Output>(
- ".y2018.control_loops.superstructure.output")),
+ test_event_loop_->MakeFetcher<superstructure::Output>(
+ "/superstructure")),
superstructure_event_loop_(MakeEventLoop()),
- superstructure_(superstructure_event_loop_.get(),
- ".y2018.control_loops.superstructure"),
+ superstructure_(superstructure_event_loop_.get(), "/superstructure"),
superstructure_plant_event_loop_(MakeEventLoop()),
superstructure_plant_(superstructure_plant_event_loop_.get()) {
set_team_id(::frc971::control_loops::testing::kTeamNumber);
@@ -319,28 +345,30 @@
ASSERT_TRUE(superstructure_goal_fetcher_.get() != nullptr) << ": No goal";
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
// Left side test.
- EXPECT_NEAR(superstructure_goal_fetcher_->intake.left_intake_angle,
- superstructure_status_fetcher_->left_intake.spring_position +
- superstructure_status_fetcher_->left_intake.motor_position,
- 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->intake.left_intake_angle,
+ EXPECT_NEAR(
+ superstructure_goal_fetcher_->intake()->left_intake_angle(),
+ superstructure_status_fetcher_->left_intake()->spring_position() +
+ superstructure_status_fetcher_->left_intake()->motor_position(),
+ 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->intake()->left_intake_angle(),
superstructure_plant_.left_intake().spring_position(), 0.001);
// Right side test.
- EXPECT_NEAR(superstructure_goal_fetcher_->intake.right_intake_angle,
- superstructure_status_fetcher_->right_intake.spring_position +
- superstructure_status_fetcher_->right_intake.motor_position,
- 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->intake.right_intake_angle,
+ EXPECT_NEAR(
+ superstructure_goal_fetcher_->intake()->right_intake_angle(),
+ superstructure_status_fetcher_->right_intake()->spring_position() +
+ superstructure_status_fetcher_->right_intake()->motor_position(),
+ 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->intake()->right_intake_angle(),
superstructure_plant_.right_intake().spring_position(), 0.001);
}
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
- ::aos::Fetcher<SuperstructureQueue::Goal> superstructure_goal_fetcher_;
- ::aos::Sender<SuperstructureQueue::Goal> superstructure_goal_sender_;
- ::aos::Fetcher<SuperstructureQueue::Status> superstructure_status_fetcher_;
- ::aos::Fetcher<SuperstructureQueue::Output> superstructure_output_fetcher_;
+ ::aos::Fetcher<superstructure::Goal> superstructure_goal_fetcher_;
+ ::aos::Sender<superstructure::Goal> superstructure_goal_sender_;
+ ::aos::Fetcher<superstructure::Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<superstructure::Output> superstructure_output_fetcher_;
// Create a control loop and simulation.
::std::unique_ptr<::aos::EventLoop> superstructure_event_loop_;
@@ -360,8 +388,10 @@
superstructure_.intake_left().state());
EXPECT_EQ(intake::IntakeSide::State::RUNNING,
superstructure_.intake_right().state());
- EXPECT_EQ(superstructure_output_fetcher_->left_intake.voltage_elastic, 0.0);
- EXPECT_EQ(superstructure_output_fetcher_->right_intake.voltage_elastic, 0.0);
+ EXPECT_EQ(superstructure_output_fetcher_->left_intake()->voltage_elastic(),
+ 0.0);
+ EXPECT_EQ(superstructure_output_fetcher_->right_intake()->voltage_elastic(),
+ 0.0);
}
// Tests that the intake loop can reach a goal.
@@ -369,14 +399,20 @@
SetEnabled(true);
// Set a reasonable goal.
{
- auto goal = superstructure_goal_sender_.MakeMessage();
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- goal->intake.left_intake_angle = 0.1;
- goal->intake.right_intake_angle = 0.2;
- goal->arm_goal_position = arm::UpIndex();
- goal->open_claw = true;
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(0.1);
+ intake_goal_builder.add_right_intake_angle(0.2);
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::UpIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Give it a lot of time to get there.
@@ -393,14 +429,20 @@
// Set a reasonable goal.
{
- auto goal = superstructure_goal_sender_.MakeMessage();
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- goal->intake.left_intake_angle = 0.1;
- goal->intake.right_intake_angle = 0.2;
- goal->arm_goal_position = arm::UpIndex();
- goal->open_claw = true;
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(0.1);
+ intake_goal_builder.add_right_intake_angle(0.2);
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::UpIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Give it a lot of time to get there.
@@ -415,43 +457,58 @@
SetEnabled(true);
// Set some ridiculous goals to test upper limits.
{
- auto goal = superstructure_goal_sender_.MakeMessage();
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- goal->intake.left_intake_angle = 5.0 * M_PI;
- goal->intake.right_intake_angle = 5.0 * M_PI;
- goal->arm_goal_position = arm::UpIndex();
- goal->open_claw = true;
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(5.0 * M_PI);
+ intake_goal_builder.add_right_intake_angle(5.0 * M_PI);
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::UpIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
// Check that we are near our soft limit.
superstructure_status_fetcher_.Fetch();
- EXPECT_NEAR(0.0, superstructure_status_fetcher_->left_intake.spring_position,
+ EXPECT_NEAR(0.0,
+ superstructure_status_fetcher_->left_intake()->spring_position(),
0.001);
- EXPECT_NEAR(constants::Values::kIntakeRange().upper,
- superstructure_status_fetcher_->left_intake.spring_position +
- superstructure_status_fetcher_->left_intake.motor_position,
- 0.001);
+ EXPECT_NEAR(
+ constants::Values::kIntakeRange().upper,
+ superstructure_status_fetcher_->left_intake()->spring_position() +
+ superstructure_status_fetcher_->left_intake()->motor_position(),
+ 0.001);
- EXPECT_NEAR(0.0, superstructure_status_fetcher_->right_intake.spring_position,
+ EXPECT_NEAR(0.0,
+ superstructure_status_fetcher_->right_intake()->spring_position(),
0.001);
EXPECT_NEAR(constants::Values::kIntakeRange().upper,
- superstructure_status_fetcher_->right_intake.motor_position,
+ superstructure_status_fetcher_->right_intake()->motor_position(),
0.001);
// Set some ridiculous goals to test lower limits.
{
- auto goal = superstructure_goal_sender_.MakeMessage();
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- goal->intake.left_intake_angle = -5.0 * M_PI;
- goal->intake.right_intake_angle = -5.0 * M_PI;
- goal->arm_goal_position = arm::UpIndex();
- goal->open_claw = true;
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(-5.0 * M_PI);
+ intake_goal_builder.add_right_intake_angle(-5.0 * M_PI);
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::UpIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -460,13 +517,17 @@
superstructure_status_fetcher_.Fetch();
EXPECT_NEAR(constants::Values::kIntakeRange().lower,
- superstructure_status_fetcher_->left_intake.motor_position, 0.001);
- EXPECT_NEAR(0.0, superstructure_status_fetcher_->left_intake.spring_position,
+ superstructure_status_fetcher_->left_intake()->motor_position(),
+ 0.001);
+ EXPECT_NEAR(0.0,
+ superstructure_status_fetcher_->left_intake()->spring_position(),
0.001);
EXPECT_NEAR(constants::Values::kIntakeRange().lower,
- superstructure_status_fetcher_->right_intake.motor_position, 0.001);
- EXPECT_NEAR(0.0, superstructure_status_fetcher_->right_intake.spring_position,
+ superstructure_status_fetcher_->right_intake()->motor_position(),
+ 0.001);
+ EXPECT_NEAR(0.0,
+ superstructure_status_fetcher_->right_intake()->spring_position(),
0.001);
}
@@ -475,19 +536,40 @@
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange().lower_hard);
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->intake.left_intake_angle = constants::Values::kIntakeRange().lower;
- goal->intake.right_intake_angle = constants::Values::kIntakeRange().lower;
- goal->arm_goal_position = arm::UpIndex();
- goal->open_claw = true;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(
+ constants::Values::kIntakeRange().lower);
+ intake_goal_builder.add_right_intake_angle(
+ constants::Values::kIntakeRange().lower);
+
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::UpIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->intake.left_intake_angle = 1.0;
- goal->intake.right_intake_angle = 1.0;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(1.0);
+ intake_goal_builder.add_right_intake_angle(1.0);
+
+ flatbuffers::Offset<IntakeGoal> intake_offset =
+ intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::UpIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
VerifyNearGoal();
@@ -499,12 +581,22 @@
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange().upper_hard);
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->intake.left_intake_angle = constants::Values::kIntakeRange().upper;
- goal->intake.right_intake_angle = constants::Values::kIntakeRange().upper;
- goal->arm_goal_position = arm::UpIndex();
- goal->open_claw = true;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(
+ constants::Values::kIntakeRange().upper);
+ intake_goal_builder.add_right_intake_angle(
+ constants::Values::kIntakeRange().upper);
+
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::UpIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -517,14 +609,22 @@
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange().upper);
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->intake.left_intake_angle =
- constants::Values::kIntakeRange().upper - 0.1;
- goal->intake.right_intake_angle =
- constants::Values::kIntakeRange().upper - 0.1;
- goal->arm_goal_position = arm::UpIndex();
- goal->open_claw = true;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(
+ constants::Values::kIntakeRange().upper - 0.1);
+ intake_goal_builder.add_right_intake_angle(
+ constants::Values::kIntakeRange().upper - 0.1);
+
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::UpIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -558,12 +658,20 @@
RunFor(chrono::seconds(5));
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->intake.left_intake_angle = -0.8;
- goal->intake.right_intake_angle = -0.8;
- goal->arm_goal_position = arm::FrontHighBoxIndex();
- goal->open_claw = true;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(-0.8);
+ intake_goal_builder.add_right_intake_angle(-0.8);
+
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::FrontHighBoxIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
EXPECT_EQ(arm::Arm::State::RUNNING, superstructure_.arm().state());
@@ -573,12 +681,20 @@
TEST_F(SuperstructureTest, ArmMoveAndMoveBack) {
SetEnabled(true);
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->intake.left_intake_angle = 0.0;
- goal->intake.right_intake_angle = 0.0;
- goal->arm_goal_position = arm::FrontHighBoxIndex();
- goal->open_claw = true;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(0.0);
+ intake_goal_builder.add_right_intake_angle(0.0);
+
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::FrontHighBoxIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -586,12 +702,20 @@
VerifyNearGoal();
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->intake.left_intake_angle = 0.0;
- goal->intake.right_intake_angle = 0.0;
- goal->arm_goal_position = arm::ReadyAboveBoxIndex();
- goal->open_claw = true;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(0.0);
+ intake_goal_builder.add_right_intake_angle(0.0);
+
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::ReadyAboveBoxIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -604,12 +728,20 @@
superstructure_plant_.InitializeArmPosition(arm::ReadyAboveBoxPoint());
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->intake.left_intake_angle = 0.0;
- goal->intake.right_intake_angle = 0.0;
- goal->arm_goal_position = arm::BackLowBoxIndex();
- goal->open_claw = true;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(0.0);
+ intake_goal_builder.add_right_intake_angle(0.0);
+
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::BackLowBoxIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -617,12 +749,20 @@
VerifyNearGoal();
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->intake.left_intake_angle = 0.0;
- goal->intake.right_intake_angle = 0.0;
- goal->arm_goal_position = arm::ReadyAboveBoxIndex();
- goal->open_claw = true;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(0.0);
+ intake_goal_builder.add_right_intake_angle(0.0);
+
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::ReadyAboveBoxIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
diff --git a/y2018/control_loops/superstructure/superstructure_main.cc b/y2018/control_loops/superstructure/superstructure_main.cc
index 789f13f..3200ead 100644
--- a/y2018/control_loops/superstructure/superstructure_main.cc
+++ b/y2018/control_loops/superstructure/superstructure_main.cc
@@ -1,12 +1,15 @@
#include "y2018/control_loops/superstructure/superstructure.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
int main() {
::aos::InitNRT(true);
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::y2018::control_loops::superstructure::Superstructure superstructure(
&event_loop);
diff --git a/y2018/control_loops/superstructure/superstructure_output.fbs b/y2018/control_loops/superstructure/superstructure_output.fbs
new file mode 100644
index 0000000..f4d62da
--- /dev/null
+++ b/y2018/control_loops/superstructure/superstructure_output.fbs
@@ -0,0 +1,40 @@
+namespace y2018.control_loops.superstructure;
+
+table IntakeVoltage {
+ // Voltage of the motors on the series elastic on one side (left or right) of
+ // the intake.
+ voltage_elastic:double;
+
+ // Voltage of the rollers on one side (left or right) of the intake.
+ voltage_rollers:double;
+}
+
+table Output {
+ // Voltage sent to the parts on the left side of the intake.
+ left_intake:IntakeVoltage;
+
+ // Voltage sent to the parts on the right side of the intake.
+ right_intake:IntakeVoltage;
+
+ // Voltage sent to the motors on the proximal joint of the arm.
+ voltage_proximal:double;
+
+ // Voltage sent to the motors on the distal joint of the arm.
+ voltage_distal:double;
+
+ // Voltage sent to the hanger. Positive pulls the robot up.
+ voltage_winch:double;
+
+ // Clamped (when true) or unclamped (when false) status sent to the
+ // pneumatic claw on the arm.
+ claw_grabbed:bool;
+
+ // If true, release the arm brakes.
+ release_arm_brake:bool;
+ // If true, release the hook
+ hook_release:bool;
+ // If true, release the forks
+ forks_release:bool;
+}
+
+root_type Output;
diff --git a/y2018/control_loops/superstructure/superstructure_position.fbs b/y2018/control_loops/superstructure/superstructure_position.fbs
new file mode 100644
index 0000000..0323b78
--- /dev/null
+++ b/y2018/control_loops/superstructure/superstructure_position.fbs
@@ -0,0 +1,50 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2018.control_loops.superstructure;
+
+table IntakeElasticSensors {
+ // Position of the motor end of the series elastic in radians.
+ motor_position:frc971.PotAndAbsolutePosition;
+
+ // Displacement of the spring in radians.
+ spring_angle:double;
+
+ // False if the beam break sensor isn't triggered, true if the beam breaker is
+ // triggered.
+ beam_break:bool;
+}
+
+table ArmPosition {
+ // Values of the encoder and potentiometer at the base of the proximal
+ // (connected to drivebase) arm in radians.
+ proximal:frc971.PotAndAbsolutePosition;
+
+ // Values of the encoder and potentiometer at the base of the distal
+ // (connected to proximal) arm in radians.
+ distal:frc971.PotAndAbsolutePosition;
+}
+
+
+table Position {
+ // Values of the series elastic encoders on the left side of the robot from
+ // the rear perspective in radians.
+ left_intake:IntakeElasticSensors;
+
+ // Values of the series elastic encoders on the right side of the robot from
+ // the rear perspective in radians.
+ right_intake:IntakeElasticSensors;
+
+ arm:ArmPosition;
+
+ // Value of the beam breaker sensor. This value is true if the beam is
+ // broken, false if the beam isn't broken.
+ claw_beambreak_triggered:bool;
+ // Value of the beambreak sensor detecting when the box has hit the frame
+ // cutout.
+ box_back_beambreak_triggered:bool;
+
+ // Distance to the box in meters.
+ box_distance:double;
+}
+
+root_type Position;
diff --git a/y2018/control_loops/superstructure/superstructure_status.fbs b/y2018/control_loops/superstructure/superstructure_status.fbs
new file mode 100644
index 0000000..af2d1ab
--- /dev/null
+++ b/y2018/control_loops/superstructure/superstructure_status.fbs
@@ -0,0 +1,98 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2018.control_loops.superstructure;
+
+table IntakeSideStatus {
+ // Is the subsystem zeroed?
+ zeroed:bool;
+
+ // The state of the subsystem, if applicable.
+ state:int;
+
+ // If true, we have aborted.
+ estopped:bool;
+
+ // Estimated position of the spring.
+ spring_position:float;
+ // Estimated velocity of the spring in units/second.
+ spring_velocity:float;
+
+ // Estimated position of the joint.
+ motor_position:float;
+ // Estimated velocity of the joint in units/second.
+ motor_velocity:float;
+
+ // Goal position of the joint.
+ goal_position:float;
+ // Goal velocity of the joint in units/second.
+ goal_velocity:float;
+
+ // The calculated velocity with delta x/delta t
+ calculated_velocity:float;
+
+ // The voltage given last cycle;
+ delayed_voltage:float;
+
+ // State of the estimator.
+ estimator_state:frc971.PotAndAbsoluteEncoderEstimatorState;
+}
+
+table ArmStatus {
+ // State of the estimators.
+ proximal_estimator_state:frc971.PotAndAbsoluteEncoderEstimatorState;
+ distal_estimator_state:frc971.PotAndAbsoluteEncoderEstimatorState;
+
+ // The node we are currently going to.
+ current_node:uint;
+ // Distance (in radians) to the end of the path.
+ path_distance_to_go:float;
+ // Goal position and velocity (radians)
+ goal_theta0:float;
+ goal_theta1:float;
+ goal_omega0:float;
+ goal_omega1:float;
+
+ // Current position and velocity (radians)
+ theta0:float;
+ theta1:float;
+
+ omega0:float;
+ omega1:float;
+
+ // Estimated voltage error for the two joints.
+ voltage_error0:float;
+ voltage_error1:float;
+
+ // True if we are zeroed.
+ zeroed:bool;
+
+ // True if the arm is zeroed.
+ estopped:bool;
+
+ // The current state machine state.
+ state:uint;
+
+ grab_state:uint;
+
+ // The number of times the LQR solver failed.
+ failed_solutions:uint;
+}
+
+table Status {
+ // Are all the subsystems zeroed?
+ zeroed:bool;
+
+ // If true, any of the subsystems have aborted.
+ estopped:bool;
+
+ // Status of both intake sides.
+ left_intake:IntakeSideStatus;
+ right_intake:IntakeSideStatus;
+
+ arm:ArmStatus;
+
+ filtered_box_velocity:double;
+ rotation_state:uint;
+}
+
+root_type Status;
diff --git a/y2018/joystick_reader.cc b/y2018/joystick_reader.cc
index 54c5f8d..c9778b8 100644
--- a/y2018/joystick_reader.cc
+++ b/y2018/joystick_reader.cc
@@ -6,22 +6,22 @@
#include <google/protobuf/stubs/stringprintf.h>
#include "aos/actions/actions.h"
+#include "aos/init.h"
+#include "aos/input/action_joystick_input.h"
#include "aos/input/driver_station_data.h"
+#include "aos/input/drivetrain_input.h"
#include "aos/logging/logging.h"
#include "aos/network/team_number.h"
#include "aos/stl_mutex/stl_mutex.h"
#include "aos/time/time.h"
#include "aos/util/log_interval.h"
-#include "aos/input/drivetrain_input.h"
-#include "aos/input/action_joystick_input.h"
-#include "aos/init.h"
#include "aos/vision/events/udp.h"
-#include "frc971/autonomous/auto.q.h"
#include "frc971/autonomous/base_autonomous_actor.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2018/control_loops/drivetrain/drivetrain_base.h"
#include "y2018/control_loops/superstructure/arm/generated_graph.h"
-#include "y2018/control_loops/superstructure/superstructure.q.h"
+#include "y2018/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2018/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2018/control_loops/superstructure/superstructure_status_generated.h"
#include "y2018/vision.pb.h"
using ::aos::monotonic_clock;
@@ -95,15 +95,15 @@
{}),
superstructure_position_fetcher_(
event_loop->MakeFetcher<
- ::y2018::control_loops::SuperstructureQueue::Position>(
+ ::y2018::control_loops::superstructure::Position>(
".frc971.control_loops.superstructure_queue.position")),
superstructure_status_fetcher_(
event_loop->MakeFetcher<
- ::y2018::control_loops::SuperstructureQueue::Status>(
+ ::y2018::control_loops::superstructure::Status>(
".frc971.control_loops.superstructure_queue.status")),
superstructure_goal_sender_(
event_loop
- ->MakeSender<::y2018::control_loops::SuperstructureQueue::Goal>(
+ ->MakeSender<::y2018::control_loops::superstructure::Goal>(
".frc971.control_loops.superstructure_queue.goal")) {
const uint16_t team = ::aos::network::GetTeamNumber();
@@ -120,10 +120,10 @@
return;
}
- auto new_superstructure_goal = superstructure_goal_sender_.MakeMessage();
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- new_superstructure_goal->intake.left_intake_angle = intake_goal_;
- new_superstructure_goal->intake.right_intake_angle = intake_goal_;
+ double roller_voltage = 0.0;
+ bool trajectory_override = false;
if (data.PosEdge(kIntakeIn) || data.PosEdge(kSmallBox) ||
data.PosEdge(kIntakeClosed)) {
@@ -132,22 +132,22 @@
if (data.IsPressed(kIntakeIn)) {
// Turn on the rollers.
- new_superstructure_goal->intake.roller_voltage = 8.0;
+ roller_voltage = 8.0;
} else if (data.IsPressed(kIntakeOut)) {
// Turn off the rollers.
- new_superstructure_goal->intake.roller_voltage = -12.0;
+ roller_voltage = -12.0;
} else {
// We don't want the rollers on.
- new_superstructure_goal->intake.roller_voltage = 0.0;
+ roller_voltage = 0.0;
}
if (data.IsPressed(kSmallBox)) {
// Deploy the intake.
- if (superstructure_position_fetcher_->box_back_beambreak_triggered) {
+ if (superstructure_position_fetcher_->box_back_beambreak_triggered()) {
intake_goal_ = 0.30;
} else {
- if (new_superstructure_goal->intake.roller_voltage > 0.1 &&
- superstructure_position_fetcher_->box_distance < 0.15) {
+ if (roller_voltage > 0.1 &&
+ superstructure_position_fetcher_->box_distance() < 0.15) {
intake_goal_ = 0.18;
} else {
intake_goal_ = -0.60;
@@ -155,22 +155,22 @@
}
} else if (data.IsPressed(kIntakeClosed)) {
// Deploy the intake.
- if (superstructure_position_fetcher_->box_back_beambreak_triggered) {
+ if (superstructure_position_fetcher_->box_back_beambreak_triggered()) {
intake_goal_ = 0.30;
} else {
- if (new_superstructure_goal->intake.roller_voltage > 0.1) {
- if (superstructure_position_fetcher_->box_distance < 0.10) {
- new_superstructure_goal->intake.roller_voltage -= 3.0;
+ if (roller_voltage > 0.1) {
+ if (superstructure_position_fetcher_->box_distance() < 0.10) {
+ roller_voltage -= 3.0;
intake_goal_ = 0.22;
- } else if (superstructure_position_fetcher_->box_distance < 0.17) {
+ } else if (superstructure_position_fetcher_->box_distance() < 0.17) {
intake_goal_ = 0.13;
- } else if (superstructure_position_fetcher_->box_distance < 0.25) {
+ } else if (superstructure_position_fetcher_->box_distance() < 0.25) {
intake_goal_ = 0.05;
} else {
intake_goal_ = -0.10;
}
if (robot_velocity() < -0.1 &&
- superstructure_position_fetcher_->box_distance > 0.15) {
+ superstructure_position_fetcher_->box_distance() > 0.15) {
intake_goal_ += 0.10;
}
} else {
@@ -182,18 +182,19 @@
intake_goal_ = -3.20;
}
- if (new_superstructure_goal->intake.roller_voltage > 0.1 &&
+ if (roller_voltage > 0.1 &&
intake_goal_ > 0.0) {
- if (superstructure_position_fetcher_->box_distance < 0.10) {
- new_superstructure_goal->intake.roller_voltage -= 3.0;
+ if (superstructure_position_fetcher_->box_distance() < 0.10) {
+ roller_voltage -= 3.0;
}
- new_superstructure_goal->intake.roller_voltage += 3.0;
+ roller_voltage += 3.0;
}
// If we are disabled, stay at the node closest to where we start. This
// should remove long motions when enabled.
if (!data.GetControlBit(ControlBit::kEnabled) || never_disabled_) {
- arm_goal_position_ = superstructure_status_fetcher_->arm.current_node;
+ arm_goal_position_ =
+ superstructure_status_fetcher_->arm()->current_node();
never_disabled_ = false;
}
@@ -202,9 +203,9 @@
grab_box = true;
}
const bool near_goal =
- superstructure_status_fetcher_->arm.current_node ==
+ superstructure_status_fetcher_->arm()->current_node() ==
arm_goal_position_ &&
- superstructure_status_fetcher_->arm.path_distance_to_go < 1e-3;
+ superstructure_status_fetcher_->arm()->path_distance_to_go() < 1e-3;
if (data.IsPressed(kArmStepDown) && near_goal) {
uint32_t *front_point = ::std::find(
front_points_.begin(), front_points_.end(), arm_goal_position_);
@@ -291,50 +292,65 @@
if (data.IsPressed(kEmergencyDown)) {
arm_goal_position_ = arm::NeutralIndex();
- new_superstructure_goal->trajectory_override = true;
+ trajectory_override = true;
} else if (data.IsPressed(kEmergencyUp)) {
arm_goal_position_ = arm::UpIndex();
- new_superstructure_goal->trajectory_override = true;
+ trajectory_override = true;
} else {
- new_superstructure_goal->trajectory_override = false;
+ trajectory_override = false;
}
- new_superstructure_goal->deploy_fork =
+ const bool deploy_fork =
data.IsPressed(kArmAboveHang) && data.IsPressed(kClawOpen);
- if (new_superstructure_goal->deploy_fork) {
+ if (deploy_fork) {
intake_goal_ = -2.0;
}
+ control_loops::superstructure::IntakeGoal::Builder intake_goal_builder =
+ builder.MakeBuilder<control_loops::superstructure::IntakeGoal>();
+
+ intake_goal_builder.add_left_intake_angle(intake_goal_);
+ intake_goal_builder.add_right_intake_angle(intake_goal_);
+ intake_goal_builder.add_roller_voltage(roller_voltage);
+
+ flatbuffers::Offset<control_loops::superstructure::IntakeGoal>
+ intake_goal_offset = intake_goal_builder.Finish();
+
+ control_loops::superstructure::Goal::Builder superstructure_builder =
+ builder.MakeBuilder<control_loops::superstructure::Goal>();
+
+ superstructure_builder.add_intake(intake_goal_offset);
+ superstructure_builder.add_grab_box(grab_box);
+ superstructure_builder.add_trajectory_override(trajectory_override);
+ superstructure_builder.add_deploy_fork(deploy_fork);
+
if (data.IsPressed(kWinch)) {
AOS_LOG(INFO, "Winching\n");
- new_superstructure_goal->voltage_winch = 12.0;
+ superstructure_builder.add_voltage_winch(12.0);
} else {
- new_superstructure_goal->voltage_winch = 0.0;
+ superstructure_builder.add_voltage_winch(0.0);
}
- new_superstructure_goal->hook_release = data.IsPressed(kArmBelowHang);
+ superstructure_builder.add_hook_release(data.IsPressed(kArmBelowHang));
- new_superstructure_goal->arm_goal_position = arm_goal_position_;
+ superstructure_builder.add_arm_goal_position(arm_goal_position_);
if (data.IsPressed(kArmFrontSwitch) || data.IsPressed(kArmBackSwitch)) {
- new_superstructure_goal->open_threshold = 0.35;
+ superstructure_builder.add_open_threshold(0.35);
} else {
- new_superstructure_goal->open_threshold = 0.0;
+ superstructure_builder.add_open_threshold(0.0);
}
if ((data.IsPressed(kClawOpen) && data.IsPressed(kDriverClawOpen)) ||
data.PosEdge(kArmPickupBoxFromIntake) ||
(data.IsPressed(kClawOpen) &&
(data.IsPressed(kArmFrontSwitch) || data.IsPressed(kArmBackSwitch)))) {
- new_superstructure_goal->open_claw = true;
+ superstructure_builder.add_open_claw(true);
} else {
- new_superstructure_goal->open_claw = false;
+ superstructure_builder.add_open_claw(false);
}
- new_superstructure_goal->grab_box = grab_box;
-
- AOS_LOG_STRUCT(DEBUG, "sending goal", *new_superstructure_goal);
- if (!new_superstructure_goal.Send()) {
+ if (!builder.Send(superstructure_builder.Finish())) {
AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
}
@@ -347,11 +363,11 @@
return mode();
}
- ::aos::Fetcher<::y2018::control_loops::SuperstructureQueue::Position>
+ ::aos::Fetcher<control_loops::superstructure::Position>
superstructure_position_fetcher_;
- ::aos::Fetcher<::y2018::control_loops::SuperstructureQueue::Status>
+ ::aos::Fetcher<control_loops::superstructure::Status>
superstructure_status_fetcher_;
- ::aos::Sender<::y2018::control_loops::SuperstructureQueue::Goal>
+ ::aos::Sender<control_loops::superstructure::Goal>
superstructure_goal_sender_;
// Current goals to send to the robot.
@@ -375,7 +391,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());
::y2018::input::joysticks::Reader reader(&event_loop);
event_loop.Run();
diff --git a/y2018/status_light.fbs b/y2018/status_light.fbs
new file mode 100644
index 0000000..e51decb
--- /dev/null
+++ b/y2018/status_light.fbs
@@ -0,0 +1,10 @@
+namespace y2018;
+
+table StatusLight {
+ // How bright to make each one. 0 is off, 1 is full on.
+ red:float;
+ green:float;
+ blue:float;
+}
+
+root_type StatusLight;
diff --git a/y2018/status_light.q b/y2018/status_light.q
deleted file mode 100644
index 90d4eec..0000000
--- a/y2018/status_light.q
+++ /dev/null
@@ -1,8 +0,0 @@
-package y2018;
-
-message StatusLight {
- // How bright to make each one. 0 is off, 1 is full on.
- float red;
- float green;
- float blue;
-};
diff --git a/y2018/vision/BUILD b/y2018/vision/BUILD
index 8e5e950..7bcd402 100644
--- a/y2018/vision/BUILD
+++ b/y2018/vision/BUILD
@@ -1,4 +1,4 @@
-load("//aos/build:queues.bzl", "queue_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
cc_binary(
name = "image_streamer",
@@ -17,11 +17,12 @@
],
)
-queue_library(
- name = "vision_queue",
+flatbuffer_cc_library(
+ name = "vision_fbs",
srcs = [
- "vision.q",
+ "vision.fbs",
],
+ gen_reflections = 1,
visibility = ["//visibility:public"],
)
@@ -32,11 +33,10 @@
],
visibility = ["//visibility:public"],
deps = [
- ":vision_queue",
+ ":vision_fbs",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
"//aos/logging",
- "//aos/logging:queue_logging",
"//aos/time",
"//aos/vision/events:udp",
"//y2018:vision_proto",
diff --git a/y2018/vision/vision.fbs b/y2018/vision/vision.fbs
new file mode 100644
index 0000000..14f8a45
--- /dev/null
+++ b/y2018/vision/vision.fbs
@@ -0,0 +1,9 @@
+namespace y2018.vision;
+
+// Published on ".y2018.vision.vision_status"
+table VisionStatus {
+ high_frame_count:uint;
+ low_frame_count:uint;
+}
+
+root_type VisionStatus;
diff --git a/y2018/vision/vision.q b/y2018/vision/vision.q
deleted file mode 100644
index fb59d69..0000000
--- a/y2018/vision/vision.q
+++ /dev/null
@@ -1,7 +0,0 @@
-package y2018.vision;
-
-// Published on ".y2018.vision.vision_status"
-message VisionStatus {
- uint32_t high_frame_count;
- uint32_t low_frame_count;
-};
diff --git a/y2018/vision/vision_status.cc b/y2018/vision/vision_status.cc
index 2f358c5..fdd6ffe 100644
--- a/y2018/vision/vision_status.cc
+++ b/y2018/vision/vision_status.cc
@@ -1,13 +1,12 @@
#include <netdb.h>
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
#include "aos/time/time.h"
#include "aos/vision/events/udp.h"
#include "y2018/vision.pb.h"
-#include "y2018/vision/vision.q.h"
+#include "y2018/vision/vision_generated.h"
namespace y2018 {
namespace vision {
@@ -15,23 +14,28 @@
using aos::monotonic_clock;
int Main() {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
::aos::events::RXUdpSocket video_rx(5001);
char data[65507];
::y2018::VisionStatus status;
- ::aos::ShmEventLoop event_loop;
- ::aos::Sender<::y2018::vision::VisionStatus> vision_status_sender_ =
- event_loop.MakeSender<::y2018::vision::VisionStatus>(
- ".y2018.vision.vision_status");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
+ ::aos::Sender<VisionStatus> vision_status_sender_ =
+ event_loop.MakeSender<VisionStatus>("/superstructure");
while (true) {
const ssize_t rx_size = video_rx.Recv(data, sizeof(data));
if (rx_size > 0) {
status.ParseFromArray(data, rx_size);
- auto new_vision_status = vision_status_sender_.MakeMessage();
- new_vision_status->high_frame_count = status.high_frame_count();
- new_vision_status->low_frame_count = status.low_frame_count();
- AOS_LOG_STRUCT(DEBUG, "vision", *new_vision_status);
- if (!new_vision_status.Send()) {
+
+ auto builder = vision_status_sender_.MakeBuilder();
+ VisionStatus::Builder vision_status_builder =
+ builder.MakeBuilder<VisionStatus>();
+ vision_status_builder.add_high_frame_count(status.high_frame_count());
+ vision_status_builder.add_low_frame_count(status.low_frame_count());
+ if (!builder.Send(vision_status_builder.Finish())) {
AOS_LOG(ERROR, "Failed to send vision information\n");
}
}
diff --git a/y2018/wpilib_interface.cc b/y2018/wpilib_interface.cc
index b93d2b0..6fc4828 100644
--- a/y2018/wpilib_interface.cc
+++ b/y2018/wpilib_interface.cc
@@ -21,19 +21,17 @@
#undef ERROR
#include "aos/commonmath.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
#include "aos/make_unique.h"
-#include "aos/robot_state/robot_state.q.h"
#include "aos/time/time.h"
#include "aos/util/compiler_memory_barrier.h"
#include "aos/util/log_interval.h"
#include "aos/util/phased_loop.h"
#include "aos/util/wrapping_counter.h"
-#include "frc971/control_loops/control_loops.q.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
#include "frc971/wpilib/ADIS16448.h"
#include "frc971/wpilib/buffered_pcm.h"
#include "frc971/wpilib/buffered_solenoid.h"
@@ -42,25 +40,26 @@
#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 "y2018/constants.h"
-#include "y2018/control_loops/superstructure/superstructure.q.h"
-#include "y2018/status_light.q.h"
-#include "y2018/vision/vision.q.h"
+#include "y2018/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2018/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2018/status_light_generated.h"
+#include "y2018/vision/vision_generated.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
-using ::y2018::control_loops::SuperstructureQueue;
-using ::y2018::constants::Values;
-using ::aos::monotonic_clock;
-namespace chrono = ::std::chrono;
using aos::make_unique;
+using ::aos::monotonic_clock;
+using ::y2018::constants::Values;
+namespace chrono = ::std::chrono;
+namespace superstructure = ::y2018::control_loops::superstructure;
namespace y2018 {
namespace wpilib {
@@ -146,12 +145,12 @@
SensorReader(::aos::EventLoop *event_loop)
: ::frc971::wpilib::SensorReader(event_loop),
superstructure_position_sender_(
- event_loop->MakeSender<SuperstructureQueue::Position>(
- ".y2018.control_loops.superstructure_queue.position")),
+ event_loop->MakeSender<superstructure::Position>(
+ "/superstructure")),
drivetrain_position_sender_(
- event_loop->MakeSender<
- ::frc971::control_loops::DrivetrainQueue::Position>(
- ".frc971.control_loops.drivetrain_queue.position")) {
+ event_loop
+ ->MakeSender<::frc971::control_loops::drivetrain::Position>(
+ "/drivetrain")) {
// Set to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
@@ -272,24 +271,27 @@
void RunIteration() {
{
- 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());
- drivetrain_message->left_shifter_position =
- drivetrain_shifter_pot_translate(
- left_drivetrain_shifter_->GetVoltage());
+ auto builder = drivetrain_position_sender_.MakeBuilder();
+ frc971::control_loops::drivetrain::Position::Builder drivetrain_builder =
+ builder.MakeBuilder<frc971::control_loops::drivetrain::Position>();
- drivetrain_message->right_encoder =
- -drivetrain_translate(drivetrain_right_encoder_->GetRaw());
- drivetrain_message->right_speed =
- -drivetrain_velocity_translate(drivetrain_right_encoder_->GetPeriod());
- drivetrain_message->right_shifter_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_builder.add_left_shifter_position (
drivetrain_shifter_pot_translate(
- right_drivetrain_shifter_->GetVoltage());
+ left_drivetrain_shifter_->GetVoltage()));
- drivetrain_message.Send();
+ drivetrain_builder.add_right_encoder (
+ -drivetrain_translate(drivetrain_right_encoder_->GetRaw()));
+ drivetrain_builder.add_right_speed (
+ -drivetrain_velocity_translate(drivetrain_right_encoder_->GetPeriod()));
+ drivetrain_builder.add_right_shifter_position (
+ drivetrain_shifter_pot_translate(
+ right_drivetrain_shifter_->GetVoltage()));
+
+ builder.Send(drivetrain_builder.Finish());
}
}
@@ -297,57 +299,111 @@
const auto values = constants::GetValues();
{
- auto superstructure_message =
- superstructure_position_sender_.MakeMessage();
+ auto builder =
+ superstructure_position_sender_.MakeBuilder();
- CopyPosition(proximal_encoder_, &superstructure_message->arm.proximal,
+ // Proximal arm
+ frc971::PotAndAbsolutePositionT arm_proximal;
+ CopyPosition(proximal_encoder_, &arm_proximal,
Values::kProximalEncoderCountsPerRevolution(),
Values::kProximalEncoderRatio(), proximal_pot_translate,
true, values.arm_proximal.potentiometer_offset);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> arm_proximal_offset =
+ frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &arm_proximal);
- CopyPosition(distal_encoder_, &superstructure_message->arm.distal,
+ // Distal arm
+ frc971::PotAndAbsolutePositionT arm_distal;
+ CopyPosition(distal_encoder_, &arm_distal,
Values::kDistalEncoderCountsPerRevolution(),
Values::kDistalEncoderRatio(), distal_pot_translate, true,
values.arm_distal.potentiometer_offset);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> arm_distal_offset =
+ frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &arm_distal);
+ superstructure::ArmPosition::Builder arm_position_builder =
+ builder.MakeBuilder<superstructure::ArmPosition>();
+ arm_position_builder.add_proximal(arm_proximal_offset);
+ arm_position_builder.add_distal(arm_distal_offset);
+
+ flatbuffers::Offset<superstructure::ArmPosition> arm_position_offset =
+ arm_position_builder.Finish();
+
+ // Left intake
+ frc971::PotAndAbsolutePositionT left_intake_motor_position;
CopyPosition(left_intake_encoder_,
- &superstructure_message->left_intake.motor_position,
+ &left_intake_motor_position,
Values::kIntakeMotorEncoderCountsPerRevolution(),
Values::kIntakeMotorEncoderRatio(), intake_pot_translate,
false, values.left_intake.potentiometer_offset);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition>
+ left_intake_motor_position_offset =
+ frc971::PotAndAbsolutePosition::Pack(*builder.fbb(),
+ &left_intake_motor_position);
+ // Right intake
+ frc971::PotAndAbsolutePositionT right_intake_motor_position;
CopyPosition(right_intake_encoder_,
- &superstructure_message->right_intake.motor_position,
+ &right_intake_motor_position,
Values::kIntakeMotorEncoderCountsPerRevolution(),
Values::kIntakeMotorEncoderRatio(), intake_pot_translate,
true, values.right_intake.potentiometer_offset);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition>
+ right_intake_motor_position_offset =
+ frc971::PotAndAbsolutePosition::Pack(*builder.fbb(),
+ &right_intake_motor_position);
- superstructure_message->left_intake.spring_angle =
+ superstructure::IntakeElasticSensors::Builder
+ left_intake_sensors_builder =
+ builder.MakeBuilder<superstructure::IntakeElasticSensors>();
+
+ left_intake_sensors_builder.add_motor_position(
+ left_intake_motor_position_offset);
+ left_intake_sensors_builder.add_spring_angle(
intake_spring_translate(left_intake_spring_angle_->GetVoltage()) +
- values.left_intake.spring_offset;
- superstructure_message->left_intake.beam_break =
- !left_intake_cube_detector_->Get();
+ values.left_intake.spring_offset);
+ left_intake_sensors_builder.add_beam_break(
+ !left_intake_cube_detector_->Get());
- superstructure_message->right_intake.spring_angle =
+ flatbuffers::Offset<superstructure::IntakeElasticSensors>
+ left_intake_offset = left_intake_sensors_builder.Finish();
+
+ superstructure::IntakeElasticSensors::Builder
+ right_intake_sensors_builder =
+ builder.MakeBuilder<superstructure::IntakeElasticSensors>();
+
+ right_intake_sensors_builder.add_motor_position(
+ right_intake_motor_position_offset);
+ right_intake_sensors_builder.add_spring_angle(
-intake_spring_translate(right_intake_spring_angle_->GetVoltage()) +
- values.right_intake.spring_offset;
- superstructure_message->right_intake.beam_break =
- !right_intake_cube_detector_->Get();
+ values.right_intake.spring_offset);
+ right_intake_sensors_builder.add_beam_break(
+ !right_intake_cube_detector_->Get());
- superstructure_message->claw_beambreak_triggered = !claw_beambreak_->Get();
- superstructure_message->box_back_beambreak_triggered =
- !box_back_beambreak_->Get();
+ flatbuffers::Offset<control_loops::superstructure::IntakeElasticSensors>
+ right_intake_offset = right_intake_sensors_builder.Finish();
- superstructure_message->box_distance =
- lidar_lite_.last_width() / 0.00001 / 100.0 / 2;
+ superstructure::Position::Builder superstructure_builder =
+ builder.MakeBuilder<superstructure::Position>();
- superstructure_message.Send();
+ superstructure_builder.add_left_intake(left_intake_offset);
+ superstructure_builder.add_right_intake(right_intake_offset);
+ superstructure_builder.add_arm(arm_position_offset);
+
+ superstructure_builder.add_claw_beambreak_triggered(
+ !claw_beambreak_->Get());
+ superstructure_builder.add_box_back_beambreak_triggered(
+ !box_back_beambreak_->Get());
+
+ superstructure_builder.add_box_distance(lidar_lite_.last_width() /
+ 0.00001 / 100.0 / 2);
+
+ builder.Send(superstructure_builder.Finish());
}
}
private:
- ::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_;
::std::unique_ptr<frc::AnalogInput> left_drivetrain_shifter_,
@@ -378,16 +434,16 @@
: event_loop_(event_loop),
drivetrain_fetcher_(
event_loop
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Output>(
- ".frc971.control_loops.drivetrain_queue.output")),
+ ->MakeFetcher<::frc971::control_loops::drivetrain::Output>(
+ "/drivetrain")),
superstructure_fetcher_(
- event_loop->MakeFetcher<SuperstructureQueue::Output>(
- ".y2018.control_loops.superstructure_queue.output")),
- status_light_fetcher_(event_loop->MakeFetcher<::y2018::StatusLight>(
- ".y2018.status_light")),
+ event_loop->MakeFetcher<superstructure::Output>("/superstructure")),
+ status_light_fetcher_(
+ event_loop->MakeFetcher<::y2018::StatusLight>("/superstructure")),
vision_status_fetcher_(
- event_loop->MakeFetcher<::y2018::vision::VisionStatus>(
- ".y2018.vision.vision_status")),
+ event_loop->MakeFetcher<::y2018::vision::VisionStatus>("/vision")),
+ pneumatics_to_log_sender_(
+ event_loop->MakeSender<::frc971::wpilib::PneumaticsToLog>("/aos")),
pcm_(pcm) {
event_loop_->set_name("Solenoids");
event_loop_->SetRuntimeRealtimePriority(27);
@@ -449,40 +505,40 @@
{
drivetrain_fetcher_.Fetch();
if (drivetrain_fetcher_.get()) {
- AOS_LOG_STRUCT(DEBUG, "solenoids", *drivetrain_fetcher_);
- left_drivetrain_shifter_->Set(!drivetrain_fetcher_->left_high);
- right_drivetrain_shifter_->Set(!drivetrain_fetcher_->right_high);
+ left_drivetrain_shifter_->Set(!drivetrain_fetcher_->left_high());
+ right_drivetrain_shifter_->Set(!drivetrain_fetcher_->right_high());
}
}
{
superstructure_fetcher_.Fetch();
if (superstructure_fetcher_.get()) {
- AOS_LOG_STRUCT(DEBUG, "solenoids", *superstructure_fetcher_);
-
- claw_->Set(!superstructure_fetcher_->claw_grabbed);
- arm_brakes_->Set(superstructure_fetcher_->release_arm_brake);
- hook_->Set(superstructure_fetcher_->hook_release);
- forks_->Set(superstructure_fetcher_->forks_release);
+ claw_->Set(!superstructure_fetcher_->claw_grabbed());
+ arm_brakes_->Set(superstructure_fetcher_->release_arm_brake());
+ hook_->Set(superstructure_fetcher_->hook_release());
+ forks_->Set(superstructure_fetcher_->forks_release());
}
}
{
- ::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());
}
monotonic_clock::time_point monotonic_now = event_loop_->monotonic_now();
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() ||
- monotonic_now >
- status_light_fetcher_->sent_time + chrono::milliseconds(100)) {
- StatusLight color;
+ monotonic_now > status_light_fetcher_.context().monotonic_sent_time +
+ chrono::milliseconds(100)) {
color.red = 0.0;
color.green = 0.0;
color.blue = 0.0;
@@ -493,7 +549,8 @@
color.red = 0.5;
} else if (!vision_status_fetcher_.get() ||
monotonic_now >
- vision_status_fetcher_->sent_time + chrono::seconds(1)) {
+ vision_status_fetcher_.context().monotonic_sent_time +
+ chrono::seconds(1)) {
color.red = 0.5;
color.green = 0.5;
}
@@ -501,16 +558,13 @@
if (light_flash_ > 20) {
light_flash_ = 0;
}
-
- AOS_LOG_STRUCT(DEBUG, "color", color);
- SetColor(color);
} else {
- AOS_LOG_STRUCT(DEBUG, "color", *status_light_fetcher_);
- SetColor(*status_light_fetcher_);
+ 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;
@@ -541,12 +595,14 @@
private:
::aos::EventLoop *event_loop_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Output>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Output>
drivetrain_fetcher_;
- ::aos::Fetcher<SuperstructureQueue::Output> superstructure_fetcher_;
+ ::aos::Fetcher<superstructure::Output> superstructure_fetcher_;
::aos::Fetcher<::y2018::StatusLight> status_light_fetcher_;
::aos::Fetcher<::y2018::vision::VisionStatus> vision_status_fetcher_;
+ aos::Sender<::frc971::wpilib::PneumaticsToLog> pneumatics_to_log_sender_;
+
::frc971::wpilib::BufferedPcm *pcm_;
::std::unique_ptr<::frc971::wpilib::BufferedSolenoid>
@@ -567,11 +623,11 @@
};
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, ".y2018.control_loops.superstructure_queue.output") {}
+ : ::frc971::wpilib::LoopOutputHandler<superstructure::Output>(
+ event_loop, "/superstructure") {}
void set_proximal_victor(::std::unique_ptr<::frc::VictorSP> t) {
proximal_victor_ = ::std::move(t);
@@ -600,40 +656,38 @@
}
private:
- virtual void Write(const SuperstructureQueue::Output &output) override {
- AOS_LOG_STRUCT(DEBUG, "will output", output);
-
+ virtual void Write(const superstructure::Output &output) override {
left_intake_elastic_victor_->SetSpeed(
- ::aos::Clip(-output.left_intake.voltage_elastic, -kMaxBringupPower,
+ ::aos::Clip(-output.left_intake()->voltage_elastic(), -kMaxBringupPower,
kMaxBringupPower) /
12.0);
right_intake_elastic_victor_->SetSpeed(
- ::aos::Clip(output.right_intake.voltage_elastic, -kMaxBringupPower,
+ ::aos::Clip(output.right_intake()->voltage_elastic(), -kMaxBringupPower,
kMaxBringupPower) /
12.0);
left_intake_rollers_victor_->SetSpeed(
- ::aos::Clip(-output.left_intake.voltage_rollers, -kMaxBringupPower,
+ ::aos::Clip(-output.left_intake()->voltage_rollers(), -kMaxBringupPower,
kMaxBringupPower) /
12.0);
right_intake_rollers_victor_->SetSpeed(
- ::aos::Clip(output.right_intake.voltage_rollers, -kMaxBringupPower,
+ ::aos::Clip(output.right_intake()->voltage_rollers(), -kMaxBringupPower,
kMaxBringupPower) /
12.0);
- proximal_victor_->SetSpeed(::aos::Clip(-output.voltage_proximal,
+ proximal_victor_->SetSpeed(::aos::Clip(-output.voltage_proximal(),
-kMaxBringupPower,
kMaxBringupPower) /
12.0);
- distal_victor_->SetSpeed(::aos::Clip(output.voltage_distal,
+ distal_victor_->SetSpeed(::aos::Clip(output.voltage_distal(),
-kMaxBringupPower, kMaxBringupPower) /
12.0);
- hanger_victor_->SetSpeed(
- ::aos::Clip(-output.voltage_winch, -kMaxBringupPower, kMaxBringupPower) /
- 12.0);
+ hanger_victor_->SetSpeed(::aos::Clip(-output.voltage_winch(),
+ -kMaxBringupPower, kMaxBringupPower) /
+ 12.0);
}
virtual void Stop() override {
@@ -663,19 +717,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_left_drivetrain_shifter_potentiometer(
@@ -721,7 +778,7 @@
AddLoop(&sensor_reader_event_loop);
// Thread 4.
- ::aos::ShmEventLoop imu_event_loop;
+ ::aos::ShmEventLoop imu_event_loop(&config.message());
auto imu_trigger = make_unique<frc::DigitalInput>(5);
::frc971::wpilib::ADIS16448 imu(&imu_event_loop, frc::SPI::Port::kOnboardCS1,
imu_trigger.get());
@@ -735,7 +792,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(
@@ -764,7 +821,7 @@
// Thread 6.
// This is a separate event loop because we want to run it at much lower
// priority.
- ::aos::ShmEventLoop solenoid_writer_event_loop;
+ ::aos::ShmEventLoop solenoid_writer_event_loop(&config.message());
::frc971::wpilib::BufferedPcm pcm;
SolenoidWriter solenoid_writer(&solenoid_writer_event_loop, &pcm);
solenoid_writer.set_left_drivetrain_shifter(pcm.MakeSolenoid(0));
diff --git a/y2018/y2018.json b/y2018/y2018.json
new file mode 100644
index 0000000..ebb6c75
--- /dev/null
+++ b/y2018/y2018.json
@@ -0,0 +1,44 @@
+{
+ "channels":
+ [
+ {
+ "name": "/superstructure",
+ "type": "y2018.control_loops.superstructure.Goal",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2018.control_loops.superstructure.Position",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2018.control_loops.superstructure.Status",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2018.control_loops.superstructure.Output",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2018.StatusLight",
+ "frequency": 200
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2018.vision.VisionStatus",
+ "frequency": 200
+ }
+ ],
+ "applications": [
+ {
+ "name": "drivetrain"
+ }
+ ],
+ "imports": [
+ "../aos/robot_state/robot_state_config.json",
+ "../frc971/control_loops/drivetrain/drivetrain_config.json"
+ ]
+}