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/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 5f89be1..71c7278 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -1,30 +1,117 @@
package(default_visibility = ["//visibility:public"])
-load("//aos/build:queues.bzl", "queue_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos:config.bzl", "aos_config")
load("//tools:environments.bzl", "mcu_cpus")
-load("//tools/build_rules:select.bzl", "cpu_select", "compiler_select")
+load("//tools/build_rules:select.bzl", "compiler_select", "cpu_select")
-cc_binary(
- name = "replay_drivetrain",
- srcs = [
- "replay_drivetrain.cc",
- ],
- deps = [
- ":drivetrain_queue",
- "//aos:init",
- "//aos/controls:replay_control_loop",
- "//frc971/queues:gyro",
- ],
+flatbuffer_cc_library(
+ name = "drivetrain_goal_fbs",
+ srcs = ["drivetrain_goal.fbs"],
+ compatible_with = mcu_cpus,
+ gen_reflections = 1,
+ includes = ["//frc971/control_loops:control_loops_fbs_includes"],
)
-queue_library(
- name = "drivetrain_queue",
- srcs = [
- "drivetrain.q",
+flatbuffer_cc_library(
+ name = "drivetrain_output_fbs",
+ srcs = ["drivetrain_output.fbs"],
+ compatible_with = mcu_cpus,
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "drivetrain_position_fbs",
+ srcs = ["drivetrain_position.fbs"],
+ compatible_with = mcu_cpus,
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "drivetrain_status_fbs",
+ srcs = ["drivetrain_status.fbs"],
+ compatible_with = mcu_cpus,
+ gen_reflections = 1,
+ includes = ["//frc971/control_loops:control_loops_fbs_includes"],
+)
+
+genrule(
+ name = "drivetrain_goal_float_fbs_generated",
+ srcs = ["drivetrain_goal.fbs"],
+ outs = ["drivetrain_goal_float.fbs"],
+ cmd = "cat $(SRCS) | sed 's/double/float/g' > $(OUTS)",
+ compatible_with = mcu_cpus,
+)
+
+genrule(
+ name = "drivetrain_position_float_fbs_generated",
+ srcs = ["drivetrain_position.fbs"],
+ outs = ["drivetrain_position_float.fbs"],
+ cmd = "cat $(SRCS) | sed 's/double/float/g' > $(OUTS)",
+ compatible_with = mcu_cpus,
+)
+
+genrule(
+ name = "drivetrain_output_float_fbs_generated",
+ srcs = ["drivetrain_output.fbs"],
+ outs = ["drivetrain_output_float.fbs"],
+ cmd = "cat $(SRCS) | sed 's/double/float/g' > $(OUTS)",
+ compatible_with = mcu_cpus,
+)
+
+genrule(
+ name = "drivetrain_status_float_fbs_generated",
+ srcs = ["drivetrain_status.fbs"],
+ outs = ["drivetrain_status_float.fbs"],
+ cmd = "cat $(SRCS) | sed 's/double/float/g' > $(OUTS)",
+ compatible_with = mcu_cpus,
+)
+
+flatbuffer_cc_library(
+ name = "drivetrain_goal_float_fbs",
+ srcs = ["drivetrain_goal_float.fbs"],
+ compatible_with = mcu_cpus,
+ gen_reflections = 1,
+ includes = ["//frc971/control_loops:control_loops_fbs_includes"],
+)
+
+flatbuffer_cc_library(
+ name = "drivetrain_output_float_fbs",
+ srcs = ["drivetrain_output_float.fbs"],
+ compatible_with = mcu_cpus,
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "drivetrain_position_float_fbs",
+ srcs = ["drivetrain_position_float.fbs"],
+ compatible_with = mcu_cpus,
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "drivetrain_status_float_fbs",
+ srcs = ["drivetrain_status_float.fbs"],
+ compatible_with = mcu_cpus,
+ gen_reflections = 1,
+ includes = ["//frc971/control_loops:control_loops_fbs_includes"],
+)
+
+aos_config(
+ name = "config",
+ src = "drivetrain_config.json",
+ flatbuffers = [
+ ":drivetrain_goal_fbs",
+ ":drivetrain_output_fbs",
+ ":drivetrain_status_fbs",
+ ":drivetrain_position_fbs",
+ ":localizer_fbs",
+ "//frc971/queues:gyro",
+ "//frc971/wpilib:imu_fbs",
],
+ visibility = ["//visibility:public"],
deps = [
- "//aos/controls:control_loop_queues",
- "//frc971/control_loops:queues",
+ "//aos/robot_state:config",
],
)
@@ -49,7 +136,7 @@
"//aos/util:math",
"//frc971/control_loops:c2d",
"//frc971/control_loops:runge_kutta",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -66,11 +153,10 @@
],
)
-queue_library(
- name = "localizer_queue",
- srcs = [
- "localizer.q",
- ],
+flatbuffer_cc_library(
+ name = "localizer_fbs",
+ srcs = ["localizer.fbs"],
+ gen_reflections = 1,
)
cc_library(
@@ -102,11 +188,14 @@
deps = [
":distance_spline",
":drivetrain_config",
- ":drivetrain_queue",
+ ":drivetrain_goal_fbs",
+ ":drivetrain_output_fbs",
+ ":drivetrain_status_fbs",
":spline",
":trajectory",
"//aos:init",
"//aos/util:math",
+ "//frc971/control_loops:control_loops_fbs",
],
)
@@ -120,14 +209,19 @@
],
deps = [
":drivetrain_config",
- ":drivetrain_queue",
+ ":drivetrain_goal_fbs",
+ ":drivetrain_output_fbs",
+ ":drivetrain_status_fbs",
":localizer",
"//aos:math",
"//aos/util:math",
"//frc971/control_loops:c2d",
+ "//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:dlqr",
"//frc971/control_loops:pose",
- "//third_party/eigen",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//y2019/control_loops/superstructure:superstructure_goal_fbs",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -158,19 +252,20 @@
],
deps = [
":drivetrain_config",
- ":drivetrain_queue",
+ ":drivetrain_goal_fbs",
+ ":drivetrain_output_fbs",
+ ":drivetrain_status_fbs",
":gear",
":localizer",
"//aos:math",
"//aos/controls:control_loop",
"//aos/controls:polytope",
- "//aos/logging:matrix_logging",
- "//aos/logging:queue_logging",
- "//aos/robot_state",
+ "//aos/robot_state:robot_state_fbs",
"//aos/util:log_interval",
"//aos/util:trapezoid_profile",
"//frc971:shifter_hall_effect",
"//frc971/control_loops:coerce_goal",
+ "//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:state_feedback_loop",
],
)
@@ -185,15 +280,17 @@
],
deps = [
":drivetrain_config",
- ":drivetrain_queue",
+ ":drivetrain_goal_fbs",
+ ":drivetrain_output_fbs",
+ ":drivetrain_position_fbs",
+ ":drivetrain_status_fbs",
":gear",
"//aos:math",
"//aos/controls:polytope",
- "//aos/logging:matrix_logging",
- "//aos/logging:queue_logging",
- "//aos/robot_state",
+ "//aos/robot_state:robot_state_fbs",
"//aos/util:log_interval",
"//frc971/control_loops:coerce_goal",
+ "//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:state_feedback_loop",
],
)
@@ -213,20 +310,24 @@
cc_library(
name = "polydrivetrain_uc",
srcs = [
- "drivetrain_uc.q.cc",
"polydrivetrain.cc",
],
hdrs = [
- "drivetrain_uc.q.h",
"polydrivetrain.h",
],
+ copts = ["-Wno-type-limits"],
restricted_to = mcu_cpus,
deps = [
":drivetrain_config_uc",
+ ":drivetrain_goal_float_fbs",
+ ":drivetrain_output_float_fbs",
+ ":drivetrain_position_float_fbs",
+ ":drivetrain_status_float_fbs",
":gear",
"//aos:math",
"//aos/controls:polytope_uc",
"//frc971/control_loops:coerce_goal_uc",
+ "//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:state_feedback_loop_uc",
],
)
@@ -267,21 +368,22 @@
],
deps = [
":down_estimator",
- ":drivetrain_queue",
+ ":drivetrain_goal_fbs",
+ ":drivetrain_output_fbs",
+ ":drivetrain_position_fbs",
+ ":drivetrain_status_fbs",
":gear",
":line_follow_drivetrain",
":localizer",
- ":localizer_queue",
+ ":localizer_fbs",
":polydrivetrain",
":splinedrivetrain",
":ssdrivetrain",
"//aos/controls:control_loop",
- "//aos/logging:matrix_logging",
- "//aos/logging:queue_logging",
"//aos/util:log_interval",
"//frc971/control_loops:runge_kutta",
"//frc971/queues:gyro",
- "//frc971/wpilib:imu_queue",
+ "//frc971/wpilib:imu_fbs",
],
)
@@ -292,10 +394,14 @@
hdrs = ["drivetrain_test_lib.h"],
deps = [
":drivetrain_config",
- ":drivetrain_queue",
+ ":drivetrain_goal_fbs",
+ ":drivetrain_output_fbs",
+ ":drivetrain_position_fbs",
+ ":drivetrain_status_fbs",
":trajectory",
- "//aos/events:event-loop",
+ "//aos/events:event_loop",
"//aos/testing:googletest",
+ "//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:state_feedback_loop",
"//frc971/queues:gyro",
"//y2016:constants",
@@ -308,6 +414,7 @@
srcs = [
"drivetrain_lib_test.cc",
],
+ data = ["config.json"],
defines =
cpu_select({
"amd64": [
@@ -319,10 +426,12 @@
deps = [
":drivetrain_config",
":drivetrain_lib",
- ":localizer_queue",
- ":drivetrain_queue",
+ ":localizer_fbs",
+ ":drivetrain_goal_fbs",
+ ":drivetrain_status_fbs",
+ ":drivetrain_position_fbs",
+ ":drivetrain_output_fbs",
":drivetrain_test_lib",
- "//aos:queues",
"//aos/controls:control_loop_test",
"//aos/testing:googletest",
"//frc971/queues:gyro",
@@ -399,7 +508,7 @@
hdrs = ["spline.h"],
deps = [
"//frc971/control_loops:binomial",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -413,8 +522,8 @@
":trajectory",
"//aos/logging:implementations",
"//aos/network:team_number",
- "//third_party/eigen",
"//y2019/control_loops/drivetrain:drivetrain_base",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -440,7 +549,7 @@
":spline",
"//aos/logging",
"//frc971/control_loops:fixed_quadrature",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -477,13 +586,12 @@
deps = [
":distance_spline",
":drivetrain_config",
- "//aos/logging:matrix_logging",
"//frc971/control_loops:c2d",
"//frc971/control_loops:dlqr",
"//frc971/control_loops:hybrid_state_feedback_loop",
"//frc971/control_loops:runge_kutta",
"//frc971/control_loops:state_feedback_loop",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -497,12 +605,11 @@
":distance_spline",
":trajectory",
"//aos/logging:implementations",
- "//aos/logging:matrix_logging",
"//aos/network:team_number",
- "//third_party/eigen",
"//third_party/matplotlib-cpp",
"//y2019/control_loops/drivetrain:drivetrain_base",
"@com_github_gflags_gflags//:gflags",
+ "@org_tuxfamily_eigen//:eigen",
],
)
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 4685ed6..78cb378 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -7,18 +7,18 @@
#include "Eigen/Dense"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
-#include "aos/logging/matrix_logging.h"
-
#include "frc971/control_loops/drivetrain/down_estimator.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/drivetrain/polydrivetrain.h"
#include "frc971/control_loops/drivetrain/ssdrivetrain.h"
#include "frc971/control_loops/runge_kutta.h"
-#include "frc971/queues/gyro.q.h"
+#include "frc971/queues/gyro_generated.h"
#include "frc971/shifter_hall_effect.h"
-#include "frc971/wpilib/imu.q.h"
+#include "frc971/wpilib/imu_generated.h"
using ::aos::monotonic_clock;
namespace chrono = ::std::chrono;
@@ -31,16 +31,16 @@
::aos::EventLoop *event_loop,
LocalizerInterface *localizer,
const ::std::string &name)
- : aos::controls::ControlLoop<::frc971::control_loops::DrivetrainQueue>(
- event_loop, name),
+ : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+ name),
dt_config_(dt_config),
- localizer_control_fetcher_(event_loop->MakeFetcher<LocalizerControl>(
- ".frc971.control_loops.drivetrain.localizer_control")),
+ localizer_control_fetcher_(
+ event_loop->MakeFetcher<LocalizerControl>("/drivetrain")),
imu_values_fetcher_(
- event_loop->MakeFetcher<::frc971::IMUValues>(".frc971.imu_values")),
+ event_loop->MakeFetcher<::frc971::IMUValues>("/drivetrain")),
gyro_reading_fetcher_(
event_loop->MakeFetcher<::frc971::sensors::GyroReading>(
- ".frc971.sensors.gyro_reading")),
+ "/drivetrain")),
localizer_(localizer),
kf_(dt_config_.make_kf_drivetrain_loop()),
dt_openloop_(dt_config_, &kf_),
@@ -89,10 +89,9 @@
}
void DrivetrainLoop::RunIteration(
- const ::frc971::control_loops::DrivetrainQueue::Goal *goal,
- const ::frc971::control_loops::DrivetrainQueue::Position *position,
- ::frc971::control_loops::DrivetrainQueue::Output *output,
- ::frc971::control_loops::DrivetrainQueue::Status *status) {
+ const drivetrain::Goal *goal, const drivetrain::Position *position,
+ aos::Sender<drivetrain::Output>::Builder *output,
+ aos::Sender<drivetrain::Status>::Builder *status) {
const monotonic_clock::time_point monotonic_now =
event_loop()->monotonic_now();
@@ -118,9 +117,9 @@
}
break;
case ShifterType::HALL_EFFECT_SHIFTER:
- left_gear_ = ComputeGear(position->left_shifter_position,
+ left_gear_ = ComputeGear(position->left_shifter_position(),
dt_config_.left_drive, left_high_requested_);
- right_gear_ = ComputeGear(position->right_shifter_position,
+ right_gear_ = ComputeGear(position->right_shifter_position(),
dt_config_.right_drive, right_high_requested_);
break;
case ShifterType::NO_SHIFTER:
@@ -129,35 +128,39 @@
kf_.set_index(ControllerIndexFromGears());
+ flatbuffers::Offset<GearLogging> gear_logging_offset;
// Set the gear-logging parts of the status
if (status) {
- status->gear_logging.left_state = static_cast<uint32_t>(left_gear_);
- status->gear_logging.right_state = static_cast<uint32_t>(right_gear_);
- status->gear_logging.left_loop_high = MaybeHigh(left_gear_);
- status->gear_logging.right_loop_high = MaybeHigh(right_gear_);
- status->gear_logging.controller_index = kf_.index();
+ GearLogging::Builder gear_logging_builder =
+ status->MakeBuilder<GearLogging>();
+ gear_logging_builder.add_left_state(static_cast<uint32_t>(left_gear_));
+ gear_logging_builder.add_right_state(static_cast<uint32_t>(right_gear_));
+ gear_logging_builder.add_left_loop_high(MaybeHigh(left_gear_));
+ gear_logging_builder.add_right_loop_high(MaybeHigh(right_gear_));
+ gear_logging_builder.add_controller_index(kf_.index());
+ gear_logging_offset = gear_logging_builder.Finish();
}
const bool is_latest_imu_values = imu_values_fetcher_.Fetch();
if (is_latest_imu_values) {
- const double rate = -imu_values_fetcher_->gyro_y;
+ const double rate = -imu_values_fetcher_->gyro_y();
const double accel_squared =
- ::std::pow(imu_values_fetcher_->accelerometer_x, 2.0) +
- ::std::pow(imu_values_fetcher_->accelerometer_y, 2.0) +
- ::std::pow(imu_values_fetcher_->accelerometer_z, 2.0);
- const double angle = ::std::atan2(imu_values_fetcher_->accelerometer_x,
- imu_values_fetcher_->accelerometer_z) +
+ ::std::pow(imu_values_fetcher_->accelerometer_x(), 2.0) +
+ ::std::pow(imu_values_fetcher_->accelerometer_y(), 2.0) +
+ ::std::pow(imu_values_fetcher_->accelerometer_z(), 2.0);
+ const double angle = ::std::atan2(imu_values_fetcher_->accelerometer_x(),
+ imu_values_fetcher_->accelerometer_z()) +
0.008;
switch (dt_config_.imu_type) {
case IMUType::IMU_X:
- last_accel_ = -imu_values_fetcher_->accelerometer_x;
+ last_accel_ = -imu_values_fetcher_->accelerometer_x();
break;
case IMUType::IMU_FLIPPED_X:
- last_accel_ = imu_values_fetcher_->accelerometer_x;
+ last_accel_ = imu_values_fetcher_->accelerometer_x();
break;
case IMUType::IMU_Y:
- last_accel_ = -imu_values_fetcher_->accelerometer_y;
+ last_accel_ = -imu_values_fetcher_->accelerometer_y();
break;
}
@@ -185,43 +188,37 @@
switch (dt_config_.gyro_type) {
case GyroType::IMU_X_GYRO:
if (is_latest_imu_values) {
- AOS_LOG_STRUCT(DEBUG, "using", *imu_values_fetcher_.get());
- last_gyro_rate_ = imu_values_fetcher_->gyro_x;
+ last_gyro_rate_ = imu_values_fetcher_->gyro_x();
last_gyro_time_ = monotonic_now;
}
break;
case GyroType::IMU_Y_GYRO:
if (is_latest_imu_values) {
- AOS_LOG_STRUCT(DEBUG, "using", *imu_values_fetcher_.get());
- last_gyro_rate_ = imu_values_fetcher_->gyro_y;
+ last_gyro_rate_ = imu_values_fetcher_->gyro_y();
last_gyro_time_ = monotonic_now;
}
break;
case GyroType::IMU_Z_GYRO:
if (is_latest_imu_values) {
- AOS_LOG_STRUCT(DEBUG, "using", *imu_values_fetcher_.get());
- last_gyro_rate_ = imu_values_fetcher_->gyro_z;
+ last_gyro_rate_ = imu_values_fetcher_->gyro_z();
last_gyro_time_ = monotonic_now;
}
break;
case GyroType::FLIPPED_IMU_Z_GYRO:
if (is_latest_imu_values) {
- AOS_LOG_STRUCT(DEBUG, "using", *imu_values_fetcher_.get());
- last_gyro_rate_ = -imu_values_fetcher_->gyro_z;
+ last_gyro_rate_ = -imu_values_fetcher_->gyro_z();
last_gyro_time_ = monotonic_now;
}
break;
case GyroType::SPARTAN_GYRO:
if (gyro_reading_fetcher_.Fetch()) {
- AOS_LOG_STRUCT(DEBUG, "using", *gyro_reading_fetcher_.get());
- last_gyro_rate_ = gyro_reading_fetcher_->velocity;
+ last_gyro_rate_ = gyro_reading_fetcher_->velocity();
last_gyro_time_ = monotonic_now;
}
break;
case GyroType::FLIPPED_SPARTAN_GYRO:
if (gyro_reading_fetcher_.Fetch()) {
- AOS_LOG_STRUCT(DEBUG, "using", *gyro_reading_fetcher_.get());
- last_gyro_rate_ = -gyro_reading_fetcher_->velocity;
+ last_gyro_rate_ = -gyro_reading_fetcher_->velocity();
last_gyro_time_ = monotonic_now;
}
break;
@@ -236,7 +233,7 @@
{
Eigen::Matrix<double, 4, 1> Y;
- Y << position->left_encoder, position->right_encoder, last_gyro_rate_,
+ Y << position->left_encoder(), position->right_encoder(), last_gyro_rate_,
last_accel_;
kf_.Correct(Y);
// If we get a new message setting the absolute position, then reset the
@@ -244,33 +241,36 @@
// TODO(james): Use a watcher (instead of a fetcher) once we support it in
// simulation.
if (localizer_control_fetcher_.Fetch()) {
- AOS_LOG_STRUCT(DEBUG, "localizer_control", *localizer_control_fetcher_);
+ VLOG(1) << "localizer_control "
+ << aos::FlatbufferToJson(localizer_control_fetcher_.get());
localizer_->ResetPosition(
- monotonic_now, localizer_control_fetcher_->x,
- localizer_control_fetcher_->y, localizer_control_fetcher_->theta,
- localizer_control_fetcher_->theta_uncertainty,
- !localizer_control_fetcher_->keep_current_theta);
+ monotonic_now, localizer_control_fetcher_->x(),
+ localizer_control_fetcher_->y(), localizer_control_fetcher_->theta(),
+ localizer_control_fetcher_->theta_uncertainty(),
+ !localizer_control_fetcher_->keep_current_theta());
}
localizer_->Update({last_last_left_voltage_, last_last_right_voltage_},
- monotonic_now, position->left_encoder,
- position->right_encoder, last_gyro_rate_, last_accel_);
+ monotonic_now, position->left_encoder(),
+ position->right_encoder(), last_gyro_rate_, last_accel_);
}
dt_openloop_.SetPosition(position, left_gear_, right_gear_);
- int controller_type = 0;
+ ControllerType controller_type = ControllerType_POLYDRIVE;
if (goal) {
- controller_type = goal->controller_type;
+ controller_type = goal->controller_type();
- dt_closedloop_.SetGoal(*goal);
- dt_openloop_.SetGoal(*goal);
- dt_spline_.SetGoal(*goal);
- dt_line_follow_.SetGoal(monotonic_now, *goal);
+ dt_closedloop_.SetGoal(goal);
+ dt_openloop_.SetGoal(goal->wheel(), goal->throttle(), goal->quickturn(),
+ goal->highgear());
+ dt_spline_.SetGoal(goal);
+ dt_line_follow_.SetGoal(monotonic_now, goal);
}
- dt_openloop_.Update(robot_state().voltage_battery);
+ dt_openloop_.Update(robot_state().voltage_battery());
- dt_closedloop_.Update(output != NULL && controller_type == 1);
+ dt_closedloop_.Update(output != nullptr &&
+ controller_type == ControllerType_MOTION_PROFILE);
const Eigen::Matrix<double, 5, 1> trajectory_state =
(Eigen::Matrix<double, 5, 1>() << localizer_->x(), localizer_->y(),
@@ -278,25 +278,30 @@
localizer_->right_velocity())
.finished();
- dt_spline_.Update(output != NULL && controller_type == 2, trajectory_state);
+ dt_spline_.Update(
+ output != nullptr && controller_type == ControllerType_SPLINE_FOLLOWER,
+ trajectory_state);
dt_line_follow_.Update(monotonic_now, trajectory_state);
+ OutputT output_struct;
+
switch (controller_type) {
- case 0:
- dt_openloop_.SetOutput(output);
+ case ControllerType_POLYDRIVE:
+ dt_openloop_.SetOutput(output != nullptr ? &output_struct : nullptr);
break;
- case 1:
- dt_closedloop_.SetOutput(output);
+ case ControllerType_MOTION_PROFILE:
+ dt_closedloop_.SetOutput(output != nullptr ? &output_struct : nullptr);
break;
- case 2:
- dt_spline_.SetOutput(output);
+ case ControllerType_SPLINE_FOLLOWER:
+ dt_spline_.SetOutput(output != nullptr ? &output_struct : nullptr);
break;
- case 3:
- if (!dt_line_follow_.SetOutput(output)) {
+ case ControllerType_LINE_FOLLOWER:
+ if (!dt_line_follow_.SetOutput(output != nullptr ? &output_struct
+ : nullptr)) {
// If the line follow drivetrain was unable to execute (generally due to
// not having a target), execute the regular teleop drivetrain.
- dt_openloop_.SetOutput(output);
+ dt_openloop_.SetOutput(output != nullptr ? &output_struct : nullptr);
}
break;
}
@@ -305,8 +310,6 @@
// set the output status of the control loop state
if (status) {
- status->robot_speed = (kf_.X_hat(1) + kf_.X_hat(3)) / 2.0;
-
Eigen::Matrix<double, 2, 1> linear =
dt_config_.LeftRightToLinear(kf_.X_hat());
Eigen::Matrix<double, 2, 1> angular =
@@ -317,42 +320,60 @@
Eigen::Matrix<double, 4, 1> gyro_left_right =
dt_config_.AngularLinearToLeftRight(linear, angular);
- status->estimated_left_position = gyro_left_right(0, 0);
- status->estimated_right_position = gyro_left_right(2, 0);
+ const flatbuffers::Offset<CIMLogging> cim_logging_offset =
+ dt_openloop_.PopulateStatus(status->fbb());
- status->estimated_left_velocity = gyro_left_right(1, 0);
- status->estimated_right_velocity = gyro_left_right(3, 0);
- status->output_was_capped = dt_closedloop_.output_was_capped();
- status->uncapped_left_voltage = kf_.U_uncapped(0, 0);
- status->uncapped_right_voltage = kf_.U_uncapped(1, 0);
+ flatbuffers::Offset<LineFollowLogging> line_follow_logging_offset =
+ dt_line_follow_.PopulateStatus(status);
+ flatbuffers::Offset<TrajectoryLogging> trajectory_logging_offset =
+ dt_spline_.MakeTrajectoryLogging(status);
- status->left_voltage_error = kf_.X_hat(4);
- status->right_voltage_error = kf_.X_hat(5);
- status->estimated_angular_velocity_error = kf_.X_hat(6);
- status->estimated_heading = localizer_->theta();
+ StatusBuilder builder = status->MakeBuilder<Status>();
- status->x = localizer_->x();
- status->y = localizer_->y();
- status->theta = ::aos::math::NormalizeAngle(localizer_->theta());
+ dt_closedloop_.PopulateStatus(&builder);
- status->ground_angle = down_estimator_.X_hat(0) + dt_config_.down_offset;
+ builder.add_estimated_left_position(gyro_left_right(0, 0));
+ builder.add_estimated_right_position(gyro_left_right(2, 0));
- dt_openloop_.PopulateStatus(status);
- dt_closedloop_.PopulateStatus(status);
- dt_spline_.PopulateStatus(status);
- dt_line_follow_.PopulateStatus(status);
+ builder.add_estimated_left_velocity(gyro_left_right(1, 0));
+ builder.add_estimated_right_velocity(gyro_left_right(3, 0));
+
+ if (dt_spline_.enable()) {
+ dt_spline_.PopulateStatus(&builder);
+ } else {
+ builder.add_robot_speed((kf_.X_hat(1) + kf_.X_hat(3)) / 2.0);
+ builder.add_output_was_capped(dt_closedloop_.output_was_capped());
+ builder.add_uncapped_left_voltage(kf_.U_uncapped(0, 0));
+ builder.add_uncapped_right_voltage(kf_.U_uncapped(1, 0));
+ }
+
+ builder.add_left_voltage_error(kf_.X_hat(4));
+ builder.add_right_voltage_error(kf_.X_hat(5));
+ builder.add_estimated_angular_velocity_error(kf_.X_hat(6));
+ builder.add_estimated_heading(localizer_->theta());
+
+ builder.add_x(localizer_->x());
+ builder.add_y(localizer_->y());
+ builder.add_theta(::aos::math::NormalizeAngle(localizer_->theta()));
+
+ builder.add_ground_angle(down_estimator_.X_hat(0) + dt_config_.down_offset);
+ builder.add_cim_logging(cim_logging_offset);
+ builder.add_gear_logging(gear_logging_offset);
+ builder.add_line_follow_logging(line_follow_logging_offset);
+ builder.add_trajectory_logging(trajectory_logging_offset);
+ status->Send(builder.Finish());
}
double left_voltage = 0.0;
double right_voltage = 0.0;
if (output) {
- left_voltage = output->left_voltage;
- right_voltage = output->right_voltage;
- left_high_requested_ = output->left_high;
- right_high_requested_ = output->right_high;
+ left_voltage = output_struct.left_voltage;
+ right_voltage = output_struct.right_voltage;
+ left_high_requested_ = output_struct.left_high;
+ right_high_requested_ = output_struct.right_high;
}
- const double scalar = robot_state().voltage_battery / 12.0;
+ const double scalar = robot_state().voltage_battery() / 12.0;
left_voltage *= scalar;
right_voltage *= scalar;
@@ -375,14 +396,20 @@
last_state_ = kf_.X_hat();
kf_.UpdateObserver(U, dt_config_.dt);
+
+ if (output) {
+ output->Send(Output::Pack(*output->fbb(), &output_struct));
+ }
}
-void DrivetrainLoop::Zero(
- ::frc971::control_loops::DrivetrainQueue::Output *output) {
- output->left_voltage = 0;
- output->right_voltage = 0;
- output->left_high = dt_config_.default_high_gear;
- output->right_high = dt_config_.default_high_gear;
+flatbuffers::Offset<Output> DrivetrainLoop::Zero(
+ aos::Sender<Output>::Builder *output) {
+ Output::Builder builder = output->MakeBuilder<Output>();
+ builder.add_left_voltage(0);
+ builder.add_right_voltage(0);
+ builder.add_left_high(dt_config_.default_high_gear);
+ builder.add_right_high(dt_config_.default_high_gear);
+ return builder.Finish();
}
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 33ff770..9f6a34c 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -6,43 +6,49 @@
#include "aos/controls/control_loop.h"
#include "aos/controls/polytope.h"
#include "aos/util/log_interval.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/drivetrain/gear.h"
#include "frc971/control_loops/drivetrain/line_follow_drivetrain.h"
#include "frc971/control_loops/drivetrain/localizer.h"
-#include "frc971/control_loops/drivetrain/localizer.q.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "frc971/control_loops/drivetrain/polydrivetrain.h"
#include "frc971/control_loops/drivetrain/splinedrivetrain.h"
#include "frc971/control_loops/drivetrain/ssdrivetrain.h"
-#include "frc971/queues/gyro.q.h"
-#include "frc971/wpilib/imu.q.h"
+#include "frc971/queues/gyro_generated.h"
+#include "frc971/wpilib/imu_generated.h"
namespace frc971 {
namespace control_loops {
namespace drivetrain {
-class DrivetrainLoop : public aos::controls::ControlLoop<
- ::frc971::control_loops::DrivetrainQueue> {
+class DrivetrainLoop
+ : public aos::controls::ControlLoop<Goal, Position, Status, Output> {
public:
// Constructs a control loop which can take a Drivetrain or defaults to the
// drivetrain at frc971::control_loops::drivetrain
- explicit DrivetrainLoop(
- const DrivetrainConfig<double> &dt_config, ::aos::EventLoop *event_loop,
- LocalizerInterface *localizer,
- const ::std::string &name = ".frc971.control_loops.drivetrain_queue");
+ explicit DrivetrainLoop(const DrivetrainConfig<double> &dt_config,
+ ::aos::EventLoop *event_loop,
+ LocalizerInterface *localizer,
+ const ::std::string &name = "/drivetrain");
int ControllerIndexFromGears();
protected:
// Executes one cycle of the control loop.
void RunIteration(
- const ::frc971::control_loops::DrivetrainQueue::Goal *goal,
- const ::frc971::control_loops::DrivetrainQueue::Position *position,
- ::frc971::control_loops::DrivetrainQueue::Output *output,
- ::frc971::control_loops::DrivetrainQueue::Status *status) override;
+ const ::frc971::control_loops::drivetrain::Goal *goal,
+ const ::frc971::control_loops::drivetrain::Position *position,
+ aos::Sender<::frc971::control_loops::drivetrain::Output>::Builder *output,
+ aos::Sender<::frc971::control_loops::drivetrain::Status>::Builder *status)
+ override;
- void Zero(::frc971::control_loops::DrivetrainQueue::Output *output) override;
+ flatbuffers::Offset<drivetrain::Output> Zero(
+ aos::Sender<drivetrain::Output>::Builder *builder) override;
double last_gyro_rate_ = 0.0;
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
deleted file mode 100644
index c7a307e..0000000
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ /dev/null
@@ -1,224 +0,0 @@
-package frc971.control_loops;
-
-import "aos/controls/control_loops.q";
-import "frc971/control_loops/control_loops.q";
-
-// For logging information about what the code is doing with the shifters.
-struct GearLogging {
- // Which controller is being used.
- int8_t controller_index;
-
- // Whether each loop for the drivetrain sides is the high-gear one.
- bool left_loop_high;
- bool right_loop_high;
-
- // The states of each drivetrain shifter.
- int8_t left_state;
- int8_t right_state;
-};
-
-// For logging information about the state of the shifters.
-struct CIMLogging {
- // Whether the code thinks each drivetrain side is currently in gear.
- bool left_in_gear;
- bool right_in_gear;
-
- // The angular velocities (in rad/s, positive forward) the code thinks motors
- // on each side of the drivetrain are moving at.
- double left_motor_speed;
- double right_motor_speed;
-
- // The velocity estimates for each drivetrain side of the robot (in m/s,
- // positive forward) that can be used for shifting.
- double left_velocity;
- double right_velocity;
-};
-
-// For logging information about the state of the trajectory planning.
-struct TrajectoryLogging {
- // state of planning the trajectory.
- // 0: not currently planning
- // 1: received a multispline to plan
- // 2: Built the spline and planning.
- // 3: Finished the plan and ready to excecute.
- int8_t planning_state;
-
- // State of the spline execution.
- bool is_executing;
- // Whether we have finished the spline specified by current_spline_idx.
- bool is_executed;
-
- // The handle of the goal spline. 0 means stop requested.
- int32_t goal_spline_handle;
- // Handle of the executing spline. -1 means none requested. If there was no
- // spline executing when a spline finished optimizing, it will become the
- // current spline even if we aren't ready to start yet.
- int32_t current_spline_idx;
- // Handle of the spline that is being optimized and staged.
- int32_t planning_spline_idx;
-
- // Expected position and velocity on the spline
- float x;
- float y;
- float theta;
- float left_velocity;
- float right_velocity;
- float distance_remaining;
-};
-
-// For logging state of the line follower.
-struct LineFollowLogging {
- // Whether we are currently freezing target choice.
- bool frozen;
- // Whether we currently have a target.
- bool have_target;
- // Absolute position of the current goal.
- float x;
- float y;
- float theta;
- // Current lateral offset from line pointing straight out of the target.
- float offset;
- // Current distance from the plane of the target, in meters.
- float distance_to_target;
- // Current goal heading.
- float goal_theta;
- // Current relative heading.
- float rel_theta;
-};
-
-// Published on ".frc971.control_loops.drivetrain_queue"
-queue_group DrivetrainQueue {
- implements aos.control_loops.ControlLoop;
-
- message Goal {
- // Position of the steering wheel (positive = turning left when going
- // forwards).
- float wheel;
- float wheel_velocity;
- float wheel_torque;
-
- // Position of the throttle (positive forwards).
- float throttle;
- float throttle_velocity;
- float throttle_torque;
-
- // True to shift into high, false to shift into low.
- bool highgear;
-
- // True to activate quickturn.
- bool quickturn;
-
- // Type of controller in charge of the drivetrain.
- // 0: polydrive
- // 1: motion profiled position drive (statespace)
- // 2: spline follower
- // 3: line follower (for guiding into a target)
- uint8_t controller_type;
-
- // Position goals for each drivetrain side (in meters) when the
- // closed-loop controller is active.
- double left_goal;
- double right_goal;
-
- float max_ss_voltage;
-
- // Motion profile parameters.
- // The control loop will profile if these are all non-zero.
- .frc971.ProfileParameters linear;
- .frc971.ProfileParameters angular;
-
- // Parameters for a spline to follow. This just contains info on a spline to
- // compute. Each time this is sent, spline drivetrain will compute a new
- // spline.
- .frc971.MultiSpline spline;
-
- // Which spline to follow.
- int32_t spline_handle;
- };
-
- message Position {
- // Relative position of each drivetrain side (in meters).
- double left_encoder;
- double right_encoder;
-
- // The speed in m/s of each drivetrain side from the most recent encoder
- // pulse, or 0 if there was no edge within the last 5ms.
- double left_speed;
- double right_speed;
-
- // Position of each drivetrain shifter, scaled from 0.0 to 1.0 where smaller
- // is towards low gear.
- double left_shifter_position;
- double right_shifter_position;
-
- // Raw analog voltages of each shifter hall effect for logging purposes.
- double low_left_hall;
- double high_left_hall;
- double low_right_hall;
- double high_right_hall;
- };
-
- message Output {
- // Voltage to send to motor(s) on either side of the drivetrain.
- double left_voltage;
- double right_voltage;
-
- // Whether to set each shifter piston to high gear.
- bool left_high;
- bool right_high;
- };
-
- message Status {
- // Estimated speed of the center of the robot in m/s (positive forwards).
- double robot_speed;
-
- // Estimated relative position of each drivetrain side (in meters).
- double estimated_left_position;
- double estimated_right_position;
-
- // Estimated velocity of each drivetrain side (in m/s).
- double estimated_left_velocity;
- double estimated_right_velocity;
-
- // The voltage we wanted to send to each drivetrain side last cycle.
- double uncapped_left_voltage;
- double uncapped_right_voltage;
-
- // The voltage error for the left and right sides.
- double left_voltage_error;
- double right_voltage_error;
-
- // The profiled goal states.
- double profiled_left_position_goal;
- double profiled_right_position_goal;
- double profiled_left_velocity_goal;
- double profiled_right_velocity_goal;
-
- // The KF offset
- double estimated_angular_velocity_error;
- // The KF estimated heading.
- double estimated_heading;
-
- // xytheta of the robot.
- double x;
- double y;
- double theta;
-
- // True if the output voltage was capped last cycle.
- bool output_was_capped;
-
- // The angle of the robot relative to the ground.
- double ground_angle;
-
- // Information about shifting logic and curent gear, for logging purposes
- GearLogging gear_logging;
- CIMLogging cim_logging;
- TrajectoryLogging trajectory_logging;
- LineFollowLogging line_follow_logging;
- };
-
- queue Goal goal;
- queue Position position;
- queue Output output;
- queue Status status;
-};
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.json b/frc971/control_loops/drivetrain/drivetrain_config.json
new file mode 100644
index 0000000..dc828bb
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_config.json
@@ -0,0 +1,43 @@
+{
+ "channels":
+ [
+ {
+ "name": "/drivetrain",
+ "type": "frc971.IMUValues",
+ "frequency": 200
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.sensors.GyroReading",
+ "frequency": 200
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.Goal",
+ "frequency": 200
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.Position",
+ "frequency": 200
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.Status",
+ "frequency": 200
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.Output",
+ "frequency": 200
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.LocalizerControl",
+ "frequency": 200
+ }
+ ],
+ "imports": [
+ "../../../aos/robot_state/robot_state_config.json"
+ ]
+}
diff --git a/frc971/control_loops/drivetrain/drivetrain_goal.fbs b/frc971/control_loops/drivetrain/drivetrain_goal.fbs
new file mode 100644
index 0000000..8a67849
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_goal.fbs
@@ -0,0 +1,65 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace frc971.control_loops.drivetrain;
+
+enum ControllerType : byte {
+ POLYDRIVE,
+ MOTION_PROFILE,
+ SPLINE_FOLLOWER,
+ LINE_FOLLOWER,
+}
+
+table SplineGoal {
+ // index of the spline. Zero indicates the spline should not be computed.
+ spline_idx:int;
+
+ // Acutal spline.
+ spline:frc971.MultiSpline;
+
+ // Whether to follow the spline driving forwards or backwards.
+ drive_spline_backwards:bool;
+}
+
+table Goal {
+ // Position of the steering wheel (positive = turning left when going
+ // forwards).
+ wheel:float;
+ wheel_velocity:float;
+ wheel_torque:float;
+
+ // Position of the throttle (positive forwards).
+ throttle:float;
+ throttle_velocity:float;
+ throttle_torque:float;
+
+ // True to shift into high, false to shift into low.
+ highgear:bool;
+
+ // True to activate quickturn.
+ quickturn:bool;
+
+ // Type of controller in charge of the drivetrain.
+ controller_type:ControllerType;
+
+ // Position goals for each drivetrain side (in meters) when the
+ // closed-loop controller is active.
+ left_goal:double;
+ right_goal:double;
+
+ max_ss_voltage:float;
+
+ // Motion profile parameters.
+ // The control loop will profile if these are all non-zero.
+ linear:ProfileParameters;
+ angular:ProfileParameters;
+
+ // Parameters for a spline to follow. This just contains info on a spline to
+ // compute. Each time this is sent, spline drivetrain will compute a new
+ // spline.
+ spline:SplineGoal;
+
+ // Which spline to follow.
+ spline_handle:int;
+}
+
+root_type Goal;
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index e4de9bb..a46defd 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -5,19 +5,21 @@
#include "aos/controls/control_loop_test.h"
#include "aos/controls/polytope.h"
-#include "aos/events/event-loop.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/event_loop.h"
#include "aos/time/time.h"
#include "gflags/gflags.h"
#include "gtest/gtest.h"
#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
-#include "frc971/control_loops/drivetrain/localizer.q.h"
-#include "frc971/queues/gyro.q.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
+#include "frc971/queues/gyro_generated.h"
namespace frc971 {
namespace control_loops {
@@ -30,27 +32,21 @@
class DrivetrainTest : public ::aos::testing::ControlLoopTest {
protected:
DrivetrainTest()
- : ::aos::testing::ControlLoopTest(GetTestDrivetrainConfig().dt),
+ : ::aos::testing::ControlLoopTest(
+ aos::configuration::ReadConfig(
+ "frc971/control_loops/drivetrain/config.json"),
+ GetTestDrivetrainConfig().dt),
test_event_loop_(MakeEventLoop()),
drivetrain_goal_sender_(
- test_event_loop_
- ->MakeSender<::frc971::control_loops::DrivetrainQueue::Goal>(
- ".frc971.control_loops.drivetrain_queue.goal")),
+ test_event_loop_->MakeSender<Goal>("/drivetrain")),
drivetrain_goal_fetcher_(
- test_event_loop_
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Goal>(
- ".frc971.control_loops.drivetrain_queue.goal")),
+ test_event_loop_->MakeFetcher<Goal>("/drivetrain")),
drivetrain_status_fetcher_(
- test_event_loop_
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
- ".frc971.control_loops.drivetrain_queue.status")),
+ test_event_loop_->MakeFetcher<Status>("/drivetrain")),
drivetrain_output_fetcher_(
- test_event_loop_
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Output>(
- ".frc971.control_loops.drivetrain_queue.output")),
+ test_event_loop_->MakeFetcher<Output>("/drivetrain")),
localizer_control_sender_(
- test_event_loop_->MakeSender<LocalizerControl>(
- ".frc971.control_loops.drivetrain.localizer_control")),
+ test_event_loop_->MakeSender<LocalizerControl>("/drivetrain")),
drivetrain_event_loop_(MakeEventLoop()),
dt_config_(GetTestDrivetrainConfig()),
localizer_(drivetrain_event_loop_.get(), dt_config_),
@@ -65,9 +61,9 @@
void VerifyNearGoal() {
drivetrain_goal_fetcher_.Fetch();
- EXPECT_NEAR(drivetrain_goal_fetcher_->left_goal,
+ EXPECT_NEAR(drivetrain_goal_fetcher_->left_goal(),
drivetrain_plant_.GetLeftPosition(), 1e-3);
- EXPECT_NEAR(drivetrain_goal_fetcher_->right_goal,
+ EXPECT_NEAR(drivetrain_goal_fetcher_->right_goal(),
drivetrain_plant_.GetRightPosition(), 1e-3);
}
@@ -79,8 +75,10 @@
void VerifyNearSplineGoal() {
drivetrain_status_fetcher_.Fetch();
- const double expected_x = drivetrain_status_fetcher_->trajectory_logging.x;
- const double expected_y = drivetrain_status_fetcher_->trajectory_logging.y;
+ const double expected_x =
+ CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->x();
+ const double expected_y =
+ CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->y();
const ::Eigen::Vector2d actual = drivetrain_plant_.GetPosition();
EXPECT_NEAR(actual(0), expected_x, 2e-2);
EXPECT_NEAR(actual(1), expected_y, 2e-2);
@@ -92,7 +90,8 @@
::std::this_thread::sleep_for(::std::chrono::milliseconds(5));
RunFor(dt());
EXPECT_TRUE(drivetrain_status_fetcher_.Fetch());
- } while (drivetrain_status_fetcher_->trajectory_logging.planning_state !=
+ } while (CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())
+ ->planning_state() !=
(int8_t)SplineDrivetrain::PlanState::kPlannedTrajectory);
}
@@ -100,17 +99,18 @@
do {
RunFor(dt());
EXPECT_TRUE(drivetrain_status_fetcher_.Fetch());
- } while (!drivetrain_status_fetcher_->trajectory_logging.is_executed);
+ } while (!CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())
+ ->is_executed());
}
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
- ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Goal>
+ ::aos::Sender<::frc971::control_loops::drivetrain::Goal>
drivetrain_goal_sender_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Goal>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Goal>
drivetrain_goal_fetcher_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Output>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Output>
drivetrain_output_fetcher_;
::aos::Sender<LocalizerControl> localizer_control_sender_;
@@ -128,11 +128,12 @@
TEST_F(DrivetrainTest, ConvergesCorrectly) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 1;
- message->left_goal = -1.0;
- message->right_goal = 1.0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_MOTION_PROFILE);
+ goal_builder.add_left_goal(-1.0);
+ goal_builder.add_right_goal(1.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(2));
VerifyNearGoal();
@@ -143,11 +144,12 @@
TEST_F(DrivetrainTest, ConvergesWithVoltageError) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 1;
- message->left_goal = -1.0;
- message->right_goal = 1.0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_MOTION_PROFILE);
+ goal_builder.add_left_goal(-1.0);
+ goal_builder.add_right_goal(1.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
drivetrain_plant_.set_left_voltage_offset(1.0);
drivetrain_plant_.set_right_voltage_offset(1.0);
@@ -158,11 +160,12 @@
// Tests that it survives disabling.
TEST_F(DrivetrainTest, SurvivesDisabling) {
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 1;
- message->left_goal = -1.0;
- message->right_goal = 1.0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_MOTION_PROFILE);
+ goal_builder.add_left_goal(-1.0);
+ goal_builder.add_right_goal(1.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
for (int i = 0; i < 500; ++i) {
if (i > 20 && i < 200) {
@@ -187,20 +190,21 @@
TEST_F(DrivetrainTest, DriveStraightForward) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 1;
- message->left_goal = 4.0;
- message->right_goal = 4.0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_MOTION_PROFILE);
+ goal_builder.add_left_goal(4.0);
+ goal_builder.add_right_goal(4.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
for (int i = 0; i < 500; ++i) {
RunFor(dt());
ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
- EXPECT_NEAR(drivetrain_output_fetcher_->left_voltage,
- drivetrain_output_fetcher_->right_voltage, 1e-4);
- EXPECT_GT(drivetrain_output_fetcher_->left_voltage, -11);
- EXPECT_GT(drivetrain_output_fetcher_->right_voltage, -11);
+ EXPECT_NEAR(drivetrain_output_fetcher_->left_voltage(),
+ drivetrain_output_fetcher_->right_voltage(), 1e-4);
+ EXPECT_GT(drivetrain_output_fetcher_->left_voltage(), -11);
+ EXPECT_GT(drivetrain_output_fetcher_->right_voltage(), -11);
}
VerifyNearGoal();
}
@@ -210,17 +214,18 @@
TEST_F(DrivetrainTest, DriveAlmostStraightForward) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 1;
- message->left_goal = 4.0;
- message->right_goal = 3.9;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_MOTION_PROFILE);
+ goal_builder.add_left_goal(4.0);
+ goal_builder.add_right_goal(3.9);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
for (int i = 0; i < 500; ++i) {
RunFor(dt());
ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
- EXPECT_GT(drivetrain_output_fetcher_->left_voltage, -11);
- EXPECT_GT(drivetrain_output_fetcher_->right_voltage, -11);
+ EXPECT_GT(drivetrain_output_fetcher_->left_voltage(), -11);
+ EXPECT_GT(drivetrain_output_fetcher_->right_voltage(), -11);
}
VerifyNearGoal();
}
@@ -254,26 +259,39 @@
TEST_F(DrivetrainTest, ProfileStraightForward) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 1;
- message->left_goal = 4.0;
- message->right_goal = 4.0;
- message->linear.max_velocity = 1.0;
- message->linear.max_acceleration = 3.0;
- message->angular.max_velocity = 1.0;
- message->angular.max_acceleration = 3.0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ ProfileParameters::Builder linear_builder =
+ builder.MakeBuilder<ProfileParameters>();
+ linear_builder.add_max_velocity(1.0);
+ linear_builder.add_max_acceleration(3.0);
+ flatbuffers::Offset<ProfileParameters> linear_offset =
+ linear_builder.Finish();
+
+ ProfileParameters::Builder angular_builder =
+ builder.MakeBuilder<ProfileParameters>();
+ angular_builder.add_max_velocity(1.0);
+ angular_builder.add_max_acceleration(3.0);
+ flatbuffers::Offset<ProfileParameters> angular_offset =
+ angular_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_MOTION_PROFILE);
+ goal_builder.add_left_goal(4.0);
+ goal_builder.add_right_goal(4.0);
+ goal_builder.add_linear(linear_offset);
+ goal_builder.add_angular(angular_offset);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
while (monotonic_now() < monotonic_clock::time_point(chrono::seconds(6))) {
RunFor(dt());
ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
- EXPECT_NEAR(drivetrain_output_fetcher_->left_voltage,
- drivetrain_output_fetcher_->right_voltage, 1e-4);
- EXPECT_GT(drivetrain_output_fetcher_->left_voltage, -6);
- EXPECT_GT(drivetrain_output_fetcher_->right_voltage, -6);
- EXPECT_LT(drivetrain_output_fetcher_->left_voltage, 6);
- EXPECT_LT(drivetrain_output_fetcher_->right_voltage, 6);
+ EXPECT_NEAR(drivetrain_output_fetcher_->left_voltage(),
+ drivetrain_output_fetcher_->right_voltage(), 1e-4);
+ EXPECT_GT(drivetrain_output_fetcher_->left_voltage(), -6);
+ EXPECT_GT(drivetrain_output_fetcher_->right_voltage(), -6);
+ EXPECT_LT(drivetrain_output_fetcher_->left_voltage(), 6);
+ EXPECT_LT(drivetrain_output_fetcher_->right_voltage(), 6);
}
VerifyNearGoal();
}
@@ -282,26 +300,39 @@
TEST_F(DrivetrainTest, ProfileTurn) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 1;
- message->left_goal = -1.0;
- message->right_goal = 1.0;
- message->linear.max_velocity = 1.0;
- message->linear.max_acceleration = 3.0;
- message->angular.max_velocity = 1.0;
- message->angular.max_acceleration = 3.0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ ProfileParameters::Builder linear_builder =
+ builder.MakeBuilder<ProfileParameters>();
+ linear_builder.add_max_velocity(1.0);
+ linear_builder.add_max_acceleration(3.0);
+ flatbuffers::Offset<ProfileParameters> linear_offset =
+ linear_builder.Finish();
+
+ ProfileParameters::Builder angular_builder =
+ builder.MakeBuilder<ProfileParameters>();
+ angular_builder.add_max_velocity(1.0);
+ angular_builder.add_max_acceleration(3.0);
+ flatbuffers::Offset<ProfileParameters> angular_offset =
+ angular_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_MOTION_PROFILE);
+ goal_builder.add_left_goal(-1.0);
+ goal_builder.add_right_goal(1.0);
+ goal_builder.add_linear(linear_offset);
+ goal_builder.add_angular(angular_offset);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
while (monotonic_now() < monotonic_clock::time_point(chrono::seconds(6))) {
RunFor(dt());
ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
- EXPECT_NEAR(drivetrain_output_fetcher_->left_voltage,
- -drivetrain_output_fetcher_->right_voltage, 1e-4);
- EXPECT_GT(drivetrain_output_fetcher_->left_voltage, -6);
- EXPECT_GT(drivetrain_output_fetcher_->right_voltage, -6);
- EXPECT_LT(drivetrain_output_fetcher_->left_voltage, 6);
- EXPECT_LT(drivetrain_output_fetcher_->right_voltage, 6);
+ EXPECT_NEAR(drivetrain_output_fetcher_->left_voltage(),
+ -drivetrain_output_fetcher_->right_voltage(), 1e-4);
+ EXPECT_GT(drivetrain_output_fetcher_->left_voltage(), -6);
+ EXPECT_GT(drivetrain_output_fetcher_->right_voltage(), -6);
+ EXPECT_LT(drivetrain_output_fetcher_->left_voltage(), 6);
+ EXPECT_LT(drivetrain_output_fetcher_->right_voltage(), 6);
}
VerifyNearGoal();
}
@@ -310,15 +341,28 @@
TEST_F(DrivetrainTest, SaturatedTurnDrive) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 1;
- message->left_goal = 5.0;
- message->right_goal = 4.0;
- message->linear.max_velocity = 6.0;
- message->linear.max_acceleration = 4.0;
- message->angular.max_velocity = 2.0;
- message->angular.max_acceleration = 4.0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ ProfileParameters::Builder linear_builder =
+ builder.MakeBuilder<ProfileParameters>();
+ linear_builder.add_max_velocity(6.0);
+ linear_builder.add_max_acceleration(4.0);
+ flatbuffers::Offset<ProfileParameters> linear_offset =
+ linear_builder.Finish();
+
+ ProfileParameters::Builder angular_builder =
+ builder.MakeBuilder<ProfileParameters>();
+ angular_builder.add_max_velocity(2.0);
+ angular_builder.add_max_acceleration(4.0);
+ flatbuffers::Offset<ProfileParameters> angular_offset =
+ angular_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_MOTION_PROFILE);
+ goal_builder.add_left_goal(5.0);
+ goal_builder.add_right_goal(4.0);
+ goal_builder.add_linear(linear_offset);
+ goal_builder.add_angular(angular_offset);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
while (monotonic_now() < monotonic_clock::time_point(chrono::seconds(3))) {
@@ -333,61 +377,77 @@
TEST_F(DrivetrainTest, OpenLoopThenClosed) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 0;
- message->wheel = 0.0;
- message->throttle = 1.0;
- message->highgear = true;
- message->quickturn = false;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_POLYDRIVE);
+ goal_builder.add_wheel(0.0);
+ goal_builder.add_throttle(1.0);
+ goal_builder.add_highgear(true);
+ goal_builder.add_quickturn(false);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(1));
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 0;
- message->wheel = 0.0;
- message->throttle = -0.3;
- message->highgear = true;
- message->quickturn = false;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_POLYDRIVE);
+ goal_builder.add_wheel(0.0);
+ goal_builder.add_throttle(-0.3);
+ goal_builder.add_highgear(true);
+ goal_builder.add_quickturn(false);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(1));
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 0;
- message->wheel = 0.0;
- message->throttle = 0.0;
- message->highgear = true;
- message->quickturn = false;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_POLYDRIVE);
+ goal_builder.add_wheel(0.0);
+ goal_builder.add_throttle(0.0);
+ goal_builder.add_highgear(true);
+ goal_builder.add_quickturn(false);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 1;
- message->left_goal = 5.0;
- message->right_goal = 4.0;
- message->linear.max_velocity = 1.0;
- message->linear.max_acceleration = 2.0;
- message->angular.max_velocity = 1.0;
- message->angular.max_acceleration = 2.0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ ProfileParameters::Builder linear_builder =
+ builder.MakeBuilder<ProfileParameters>();
+ linear_builder.add_max_velocity(1.0);
+ linear_builder.add_max_acceleration(2.0);
+ flatbuffers::Offset<ProfileParameters> linear_offset =
+ linear_builder.Finish();
+
+ ProfileParameters::Builder angular_builder =
+ builder.MakeBuilder<ProfileParameters>();
+ angular_builder.add_max_velocity(1.0);
+ angular_builder.add_max_acceleration(2.0);
+ flatbuffers::Offset<ProfileParameters> angular_offset =
+ angular_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_MOTION_PROFILE);
+ goal_builder.add_left_goal(5.0);
+ goal_builder.add_right_goal(4.0);
+ goal_builder.add_linear(linear_offset);
+ goal_builder.add_angular(angular_offset);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
const auto end_time = monotonic_now() + chrono::seconds(4);
while (monotonic_now() < end_time) {
RunFor(dt());
ASSERT_TRUE(drivetrain_output_fetcher_.Fetch());
- EXPECT_GT(drivetrain_output_fetcher_->left_voltage, -6);
- EXPECT_GT(drivetrain_output_fetcher_->right_voltage, -6);
- EXPECT_LT(drivetrain_output_fetcher_->left_voltage, 6);
- EXPECT_LT(drivetrain_output_fetcher_->right_voltage, 6);
+ EXPECT_GT(drivetrain_output_fetcher_->left_voltage(), -6);
+ EXPECT_GT(drivetrain_output_fetcher_->right_voltage(), -6);
+ EXPECT_LT(drivetrain_output_fetcher_->left_voltage(), 6);
+ EXPECT_LT(drivetrain_output_fetcher_->right_voltage(), 6);
}
VerifyNearGoal();
}
@@ -396,22 +456,44 @@
TEST_F(DrivetrainTest, SplineSimple) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline.spline_idx = 1;
- message->spline.spline_count = 1;
- message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(1);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline(spline_goal_offset);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(dt());
// Send the start goal
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline_handle = 1;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(1);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryPlan();
@@ -423,22 +505,43 @@
TEST_F(DrivetrainTest, SplineSimpleBackwards) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline.spline_idx = 1;
- message->spline.spline_count = 1;
- message->spline.drive_spline_backwards = true;
- message->spline.spline_x = {{0.0, -0.25, -0.5, -0.5, -0.75, -1.0}};
- message->spline.spline_y = {{0.0, 0.0, -0.25, -0.75, -1.0, -1.0}};
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({0.0, -0.25, -0.5, -0.5, -0.75, -1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.0, -0.25, -0.75, -1.0, -1.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(1);
+ spline_goal_builder.add_drive_spline_backwards(true);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline(spline_goal_offset);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(dt());
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline_handle = 1;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(1);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryPlan();
@@ -456,7 +559,7 @@
drivetrain_status_fetcher_.Fetch();
auto actual = drivetrain_plant_.state();
const double expected_theta =
- drivetrain_status_fetcher_->trajectory_logging.theta;
+ CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->theta();
// As a sanity check, compare both against absolute angle and the spline's
// goal angle.
EXPECT_NEAR(0.0, ::aos::math::DiffAngle(actual(2), 0.0), 2e-2);
@@ -467,14 +570,35 @@
TEST_F(DrivetrainTest, SplineSingleGoal) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline.spline_idx = 1;
- message->spline.spline_count = 1;
- message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
- message->spline_handle = 1;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(1);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline(spline_goal_offset);
+ goal_builder.add_spline_handle(1);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryPlan();
@@ -486,100 +610,179 @@
TEST_F(DrivetrainTest, SplineStop) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline.spline_idx = 1;
- message->spline.spline_count = 1;
- message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
- message->spline_handle = 1;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(1);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline(spline_goal_offset);
+ goal_builder.add_spline_handle(1);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryPlan();
RunFor(chrono::milliseconds(500));
drivetrain_status_fetcher_.Fetch();
- const double goal_x = drivetrain_status_fetcher_->trajectory_logging.x;
- const double goal_y = drivetrain_status_fetcher_->trajectory_logging.y;
+ const double goal_x =
+ CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->x();
+ const double goal_y =
+ CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->y();
// Now stop.
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline_handle = 0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(2000));
// The goal shouldn't change after being stopped.
drivetrain_status_fetcher_.Fetch();
- EXPECT_NEAR(drivetrain_status_fetcher_->trajectory_logging.x, goal_x, 1e-9);
- EXPECT_NEAR(drivetrain_status_fetcher_->trajectory_logging.y, goal_y, 1e-9);
+ EXPECT_NEAR(
+ CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->x(),
+ goal_x, 1e-9);
+ EXPECT_NEAR(
+ CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->y(),
+ goal_y, 1e-9);
}
// Tests that a spline can't be restarted.
TEST_F(DrivetrainTest, SplineRestart) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline.spline_idx = 1;
- message->spline.spline_count = 1;
- message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
- message->spline_handle = 1;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(1);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline(spline_goal_offset);
+ goal_builder.add_spline_handle(1);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryPlan();
RunFor(chrono::milliseconds(500));
drivetrain_status_fetcher_.Fetch();
- const double goal_x = drivetrain_status_fetcher_->trajectory_logging.x;
- const double goal_y = drivetrain_status_fetcher_->trajectory_logging.y;
+ const double goal_x =
+ CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->x();
+ const double goal_y =
+ CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->y();
// Send a stop goal.
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline_handle = 0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(500));
// Send a restart.
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline_handle = 1;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(1);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(2000));
// The goal shouldn't change after being stopped and restarted.
drivetrain_status_fetcher_.Fetch();
- EXPECT_NEAR(drivetrain_status_fetcher_->trajectory_logging.x, goal_x, 1e-9);
- EXPECT_NEAR(drivetrain_status_fetcher_->trajectory_logging.y, goal_y, 1e-9);
+ EXPECT_NEAR(
+ CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->x(),
+ goal_x, 1e-9);
+ EXPECT_NEAR(
+ CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->y(),
+ goal_y, 1e-9);
}
// Tests that simple spline converges when it doesn't start where it thinks.
TEST_F(DrivetrainTest, SplineOffset) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline.spline_idx = 1;
- message->spline.spline_count = 1;
- message->spline.spline_x = {{0.2, 0.25, 0.5, 0.5, 0.75, 1.0}};
- message->spline.spline_y = {{0.2, 0.0, 0.25, 0.75, 1.0, 1.0}};
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({0.2, 0.25, 0.5, 0.5, 0.75, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({0.2, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(1);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline(spline_goal_offset);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(dt());
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline_handle = 1;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(1);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryPlan();
@@ -592,21 +795,43 @@
TEST_F(DrivetrainTest, SplineSideOffset) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline.spline_idx = 1;
- message->spline.spline_count = 1;
- message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- message->spline.spline_y = {{0.5, 0.0, 0.25, 0.75, 1.0, 1.0}};
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({0.5, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(1);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline(spline_goal_offset);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(dt());
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline_handle = 1;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(1);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryPlan();
@@ -618,23 +843,45 @@
TEST_F(DrivetrainTest, MultiSpline) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline.spline_idx = 1;
- message->spline.spline_count = 2;
- message->spline.spline_x = {
- {0.0, 0.25, 0.5, 0.5, 0.75, 1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
- message->spline.spline_y = {
- {0.0, 0.0, 0.25, 0.75, 1.0, 1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>(
+ {0.0, 0.25, 0.5, 0.5, 0.75, 1.0, 1.25, 1.5, 1.5, 1.25, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>(
+ {0.0, 0.0, 0.25, 0.75, 1.0, 1.0, 1.0, 1.25, 1.5, 1.75, 2.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(2);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(1);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline(spline_goal_offset);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(dt());
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline_handle = 1;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(1);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryPlan();
@@ -646,21 +893,43 @@
TEST_F(DrivetrainTest, SequentialSplines) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline.spline_idx = 1;
- message->spline.spline_count = 1;
- message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(1);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline(spline_goal_offset);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(dt());
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline_handle = 1;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(1);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryPlan();
@@ -670,22 +939,44 @@
// Second spline.
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline.spline_idx = 2;
- message->spline.spline_count = 1;
- message->spline.spline_x = {{1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
- message->spline.spline_y = {{1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({1.0, 1.25, 1.5, 1.5, 1.25, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({1.0, 1.0, 1.25, 1.5, 1.75, 2.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(2);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline(spline_goal_offset);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(dt());
// And then start it.
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline_handle = 2;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(2);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryPlan();
@@ -696,14 +987,35 @@
TEST_F(DrivetrainTest, SplineStopFirst) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline.spline_idx = 1;
- message->spline.spline_count = 1;
- message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
- message->spline_handle = 1;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(1);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(1);
+ goal_builder.add_spline(spline_goal_offset);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryPlan();
@@ -711,23 +1023,45 @@
// Stop goal
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline_handle = 0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(500));
// Second spline goal.
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline.spline_idx = 2;
- message->spline.spline_count = 1;
- message->spline.spline_x = {{1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
- message->spline.spline_y = {{1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
- message->spline_handle = 2;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({1.0, 1.25, 1.5, 1.5, 1.25, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({1.0, 1.0, 1.25, 1.5, 1.75, 2.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(2);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline(spline_goal_offset);
+ goal_builder.add_spline_handle(2);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryPlan();
@@ -740,15 +1074,35 @@
TEST_F(DrivetrainTest, CancelSplineBeforeExecuting) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline.spline_idx = 1;
- message->spline.spline_count = 1;
- message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
- // Don't start running the splane.
- message->spline_handle = 0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(1);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline(spline_goal_offset);
+ goal_builder.add_spline_handle(0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryPlan();
@@ -756,23 +1110,45 @@
// Plan another spline, but don't start it yet:
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline.spline_idx = 2;
- message->spline.spline_count = 1;
- message->spline.spline_x = {{0.0, 0.75, 1.25, 1.5, 1.25, 1.0}};
- message->spline.spline_y = {{0.0, 0.75, 1.25, 1.5, 1.75, 2.0}};
- message->spline_handle = 0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.75, 1.25, 1.5, 1.25, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.75, 1.25, 1.5, 1.75, 2.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(2);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline(spline_goal_offset);
+ goal_builder.add_spline_handle(0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryPlan();
// Now execute it.
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline_handle = 2;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(2);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryExecution();
@@ -784,35 +1160,78 @@
TEST_F(DrivetrainTest, ParallelSplines) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline.spline_idx = 1;
- message->spline.spline_count = 1;
- message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(1);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline(spline_goal_offset);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryPlan();
// Second spline goal
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->spline_handle = 1;
- message->controller_type = 2;
- message->spline.spline_idx = 2;
- message->spline.spline_count = 1;
- message->spline.spline_x = {{1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
- message->spline.spline_y = {{1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({1.0, 1.25, 1.5, 1.5, 1.25, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({1.0, 1.0, 1.25, 1.5, 1.75, 2.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(2);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline(spline_goal_offset);
+ goal_builder.add_spline_handle(1);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryExecution();
// Second start goal
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline_handle = 2;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(2);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(4000));
@@ -823,20 +1242,42 @@
TEST_F(DrivetrainTest, OnlyPlanSpline) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline.spline_idx = 1;
- message->spline.spline_count = 1;
- message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(1);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline(spline_goal_offset);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryPlan();
for (int i = 0; i < 100; ++i) {
RunFor(dt());
drivetrain_status_fetcher_.Fetch();
- EXPECT_EQ(drivetrain_status_fetcher_->trajectory_logging.planning_state,
+ EXPECT_EQ(CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())
+ ->planning_state(),
3);
::std::this_thread::sleep_for(::std::chrono::milliseconds(2));
}
@@ -847,23 +1288,45 @@
TEST_F(DrivetrainTest, SplineExecuteAfterPlan) {
SetEnabled(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline.spline_idx = 1;
- message->spline.spline_count = 1;
- message->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
- message->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.25, 0.5, 0.5, 0.75, 1.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder.fbb()->CreateVector<float>({0.0, 0.0, 0.25, 0.75, 1.0, 1.0});
+
+ MultiSpline::Builder multispline_builder =
+ builder.MakeBuilder<MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ SplineGoal::Builder spline_goal_builder = builder.MakeBuilder<SplineGoal>();
+ spline_goal_builder.add_spline_idx(1);
+ spline_goal_builder.add_drive_spline_backwards(false);
+ spline_goal_builder.add_spline(multispline_offset);
+ flatbuffers::Offset<SplineGoal> spline_goal_offset =
+ spline_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline(spline_goal_offset);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
WaitForTrajectoryPlan();
RunFor(chrono::milliseconds(2000));
// Start goal
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 2;
- message->spline_handle = 1;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(1);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(2000));
@@ -877,21 +1340,29 @@
localizer_.target_selector()->set_has_target(true);
localizer_.target_selector()->set_pose({{1.0, 1.0, 0.0}, M_PI_4});
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 3;
- message->throttle = 0.5;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_LINE_FOLLOWER);
+ goal_builder.add_throttle(0.5);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
drivetrain_status_fetcher_.Fetch();
- EXPECT_TRUE(drivetrain_status_fetcher_->line_follow_logging.frozen);
- EXPECT_TRUE(drivetrain_status_fetcher_->line_follow_logging.have_target);
- EXPECT_EQ(1.0, drivetrain_status_fetcher_->line_follow_logging.x);
- EXPECT_EQ(1.0, drivetrain_status_fetcher_->line_follow_logging.y);
- EXPECT_FLOAT_EQ(M_PI_4,
- drivetrain_status_fetcher_->line_follow_logging.theta);
+ EXPECT_TRUE(CHECK_NOTNULL(drivetrain_status_fetcher_->line_follow_logging())
+ ->frozen());
+ EXPECT_TRUE(CHECK_NOTNULL(drivetrain_status_fetcher_->line_follow_logging())
+ ->have_target());
+ EXPECT_EQ(
+ 1.0,
+ CHECK_NOTNULL(drivetrain_status_fetcher_->line_follow_logging())->x());
+ EXPECT_EQ(
+ 1.0,
+ CHECK_NOTNULL(drivetrain_status_fetcher_->line_follow_logging())->y());
+ EXPECT_FLOAT_EQ(
+ M_PI_4, CHECK_NOTNULL(drivetrain_status_fetcher_->line_follow_logging())
+ ->theta());
// Should have run off the end of the target, running along the y=x line.
EXPECT_LT(1.0, drivetrain_plant_.GetPosition().x());
@@ -906,10 +1377,11 @@
localizer_.target_selector()->set_has_target(false);
localizer_.target_selector()->set_pose({{1.0, 1.0, 0.0}, M_PI_4});
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 3;
- message->throttle = 0.5;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_controller_type(ControllerType_LINE_FOLLOWER);
+ goal_builder.add_throttle(0.5);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
@@ -926,11 +1398,13 @@
EXPECT_EQ(0.0, localizer_.y());
EXPECT_EQ(0.0, localizer_.theta());
{
- auto message = localizer_control_sender_.MakeMessage();
- message->x = 9.0;
- message->y = 7.0;
- message->theta = 1.0;
- ASSERT_TRUE(message.Send());
+ auto builder = localizer_control_sender_.MakeBuilder();
+ LocalizerControl::Builder localizer_control_builder =
+ builder.MakeBuilder<LocalizerControl>();
+ localizer_control_builder.add_x(9.0);
+ localizer_control_builder.add_y(7.0);
+ localizer_control_builder.add_theta(1.0);
+ ASSERT_TRUE(builder.Send(localizer_control_builder.Finish()));
}
RunFor(dt());
diff --git a/frc971/control_loops/drivetrain/drivetrain_output.fbs b/frc971/control_loops/drivetrain/drivetrain_output.fbs
new file mode 100644
index 0000000..da8c889
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_output.fbs
@@ -0,0 +1,13 @@
+namespace frc971.control_loops.drivetrain;
+
+table Output {
+ // Voltage to send to motor(s) on either side of the drivetrain.
+ left_voltage:double;
+ right_voltage:double;
+
+ // Whether to set each shifter piston to high gear.
+ left_high:bool;
+ right_high:bool;
+}
+
+root_type Output;
diff --git a/frc971/control_loops/drivetrain/drivetrain_position.fbs b/frc971/control_loops/drivetrain/drivetrain_position.fbs
new file mode 100644
index 0000000..900c036
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_position.fbs
@@ -0,0 +1,25 @@
+namespace frc971.control_loops.drivetrain;
+
+table Position {
+ // Relative position of each drivetrain side (in meters).
+ left_encoder:double;
+ right_encoder:double;
+
+ // The speed in m/s of each drivetrain side from the most recent encoder
+ // pulse, or 0 if there was no edge within the last 5ms.
+ left_speed:double;
+ right_speed:double;
+
+ // Position of each drivetrain shifter, scaled from 0.0 to 1.0 where smaller
+ // is towards low gear.
+ left_shifter_position:double;
+ right_shifter_position:double;
+
+ // Raw analog voltages of each shifter hall effect for logging purposes.
+ low_left_hall:double;
+ high_left_hall:double;
+ low_right_hall:double;
+ high_right_hall:double;
+}
+
+root_type Position;
diff --git a/frc971/control_loops/drivetrain/drivetrain_status.fbs b/frc971/control_loops/drivetrain/drivetrain_status.fbs
new file mode 100644
index 0000000..67134f5
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_status.fbs
@@ -0,0 +1,140 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace frc971.control_loops.drivetrain;
+
+// For logging information about what the code is doing with the shifters.
+table GearLogging {
+ // Which controller is being used.
+ controller_index:byte;
+
+ // Whether each loop for the drivetrain sides is the high-gear one.
+ left_loop_high:bool;
+ right_loop_high:bool;
+
+ // The states of each drivetrain shifter.
+ left_state:byte;
+ right_state:byte;
+}
+
+// For logging information about the state of the shifters.
+table CIMLogging {
+ // Whether the code thinks each drivetrain side is currently in gear.
+ left_in_gear:bool;
+ right_in_gear:bool;
+
+ // The angular velocities (in rad/s, positive forward) the code thinks motors
+ // on each side of the drivetrain are moving at.
+ left_motor_speed:double;
+ right_motor_speed:double;
+
+ // The velocity estimates for each drivetrain side of the robot (in m/s,
+ // positive forward) that can be used for shifting.
+ left_velocity:double;
+ right_velocity:double;
+}
+
+enum PlanningState : byte {
+ NO_PLAN,
+ BUILDING_TRAJECTORY,
+ PLANNING_TRAJECTORY,
+ PLANNED,
+}
+
+// For logging information about the state of the trajectory planning.
+table TrajectoryLogging {
+ // state of planning the trajectory.
+ planning_state:PlanningState;
+
+ // State of the spline execution.
+ is_executing:bool;
+ // Whether we have finished the spline specified by current_spline_idx.
+ is_executed:bool;
+
+ // The handle of the goal spline. 0 means stop requested.
+ goal_spline_handle:int;
+ // Handle of the executing spline. -1 means none requested. If there was no
+ // spline executing when a spline finished optimizing, it will become the
+ // current spline even if we aren't ready to start yet.
+ current_spline_idx:int;
+ // Handle of the spline that is being optimized and staged.
+ planning_spline_idx:int;
+
+ // Expected position and velocity on the spline
+ x:float;
+ y:float;
+ theta:float;
+ left_velocity:float;
+ right_velocity:float;
+ distance_remaining:float;
+}
+
+// For logging state of the line follower.
+table LineFollowLogging {
+ // Whether we are currently freezing target choice.
+ frozen:bool;
+ // Whether we currently have a target.
+ have_target:bool;
+ // Absolute position of the current goal.
+ x:float;
+ y:float;
+ theta:float;
+ // Current lateral offset from line pointing straight out of the target.
+ offset:float;
+ // Current distance from the plane of the target, in meters.
+ distance_to_target:float;
+ // Current goal heading.
+ goal_theta:float;
+ // Current relative heading.
+ rel_theta:float;
+}
+
+table Status {
+ // Estimated speed of the center of the robot in m/s (positive forwards).
+ robot_speed:double;
+
+ // Estimated relative position of each drivetrain side (in meters).
+ estimated_left_position:double;
+ estimated_right_position:double;
+
+ // Estimated velocity of each drivetrain side (in m/s).
+ estimated_left_velocity:double;
+ estimated_right_velocity:double;
+
+ // The voltage we wanted to send to each drivetrain side last cycle.
+ uncapped_left_voltage:double;
+ uncapped_right_voltage:double;
+
+ // The voltage error for the left and right sides.
+ left_voltage_error:double;
+ right_voltage_error:double;
+
+ // The profiled goal states.
+ profiled_left_position_goal:double;
+ profiled_right_position_goal:double;
+ profiled_left_velocity_goal:double;
+ profiled_right_velocity_goal:double;
+
+ // The KF offset
+ estimated_angular_velocity_error:double;
+ // The KF estimated heading.
+ estimated_heading:double;
+
+ // xytheta of the robot.
+ x:double;
+ y:double;
+ theta:double;
+
+ // True if the output voltage was capped last cycle.
+ output_was_capped:bool;
+
+ // The angle of the robot relative to the ground.
+ ground_angle:double;
+
+ // Information about shifting logic and curent gear, for logging purposes
+ gear_logging:GearLogging;
+ cim_logging:CIMLogging;
+ trajectory_logging:TrajectoryLogging;
+ line_follow_logging:LineFollowLogging;
+}
+
+root_type Status;
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
index 34c9907..8ebbb1b 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
@@ -90,23 +90,20 @@
DrivetrainSimulation::DrivetrainSimulation(
::aos::EventLoop *event_loop, const DrivetrainConfig<double> &dt_config)
: event_loop_(event_loop),
- robot_state_fetcher_(
- event_loop_->MakeFetcher<::aos::RobotState>(".aos.robot_state")),
+ robot_state_fetcher_(event_loop_->MakeFetcher<::aos::RobotState>("/aos")),
drivetrain_position_sender_(
event_loop_
- ->MakeSender<::frc971::control_loops::DrivetrainQueue::Position>(
- ".frc971.control_loops.drivetrain_queue.position")),
+ ->MakeSender<::frc971::control_loops::drivetrain::Position>(
+ "/drivetrain")),
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")),
drivetrain_status_fetcher_(
- event_loop_
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
- ".frc971.control_loops.drivetrain_queue.status")),
+ event_loop_->MakeFetcher<::frc971::control_loops::drivetrain::Status>(
+ "/drivetrain")),
gyro_reading_sender_(
event_loop->MakeSender<::frc971::sensors::GyroReading>(
- ".frc971.sensors.gyro_reading")),
+ "/drivetrain")),
dt_config_(dt_config),
drivetrain_plant_(MakePlantFromConfig(dt_config_)),
velocity_drivetrain_(
@@ -133,9 +130,9 @@
actual_y_.push_back(actual_position(1));
trajectory_x_.push_back(
- drivetrain_status_fetcher_->trajectory_logging.x);
+ drivetrain_status_fetcher_->trajectory_logging()->x());
trajectory_y_.push_back(
- drivetrain_status_fetcher_->trajectory_logging.y);
+ drivetrain_status_fetcher_->trajectory_logging()->y());
}
}
first_ = false;
@@ -159,22 +156,27 @@
const double right_encoder = GetRightPosition();
{
- ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Position>::Message
- position = drivetrain_position_sender_.MakeMessage();
- position->left_encoder = left_encoder;
- position->right_encoder = right_encoder;
- position->left_shifter_position = left_gear_high_ ? 1.0 : 0.0;
- position->right_shifter_position = right_gear_high_ ? 1.0 : 0.0;
- position.Send();
+ ::aos::Sender<::frc971::control_loops::drivetrain::Position>::Builder
+ builder = drivetrain_position_sender_.MakeBuilder();
+ frc971::control_loops::drivetrain::Position::Builder position_builder =
+ builder.MakeBuilder<frc971::control_loops::drivetrain::Position>();
+ position_builder.add_left_encoder(left_encoder);
+ position_builder.add_right_encoder(right_encoder);
+ position_builder.add_left_shifter_position(left_gear_high_ ? 1.0 : 0.0);
+ position_builder.add_right_shifter_position(right_gear_high_ ? 1.0 : 0.0);
+ builder.Send(position_builder.Finish());
}
{
- auto gyro = gyro_reading_sender_.MakeMessage();
- gyro->angle =
- (right_encoder - left_encoder) / (dt_config_.robot_radius * 2.0);
- gyro->velocity = (drivetrain_plant_.X(3, 0) - drivetrain_plant_.X(1, 0)) /
- (dt_config_.robot_radius * 2.0);
- gyro.Send();
+ auto builder = gyro_reading_sender_.MakeBuilder();
+ frc971::sensors::GyroReading::Builder gyro_builder =
+ builder.MakeBuilder<frc971::sensors::GyroReading>();
+ gyro_builder.add_angle((right_encoder - left_encoder) /
+ (dt_config_.robot_radius * 2.0));
+ gyro_builder.add_velocity(
+ (drivetrain_plant_.X(3, 0) - drivetrain_plant_.X(1, 0)) /
+ (dt_config_.robot_radius * 2.0));
+ builder.Send(gyro_builder.Finish());
}
}
@@ -184,17 +186,17 @@
last_right_position_ = drivetrain_plant_.Y(1, 0);
EXPECT_TRUE(drivetrain_output_fetcher_.Fetch());
::Eigen::Matrix<double, 2, 1> U = last_U_;
- last_U_ << drivetrain_output_fetcher_->left_voltage,
- drivetrain_output_fetcher_->right_voltage;
+ last_U_ << drivetrain_output_fetcher_->left_voltage(),
+ drivetrain_output_fetcher_->right_voltage();
{
robot_state_fetcher_.Fetch();
const double scalar = robot_state_fetcher_.get()
- ? robot_state_fetcher_->voltage_battery / 12.0
+ ? robot_state_fetcher_->voltage_battery() / 12.0
: 1.0;
last_U_ *= scalar;
}
- left_gear_high_ = drivetrain_output_fetcher_->left_high;
- right_gear_high_ = drivetrain_output_fetcher_->right_high;
+ left_gear_high_ = drivetrain_output_fetcher_->left_high();
+ right_gear_high_ = drivetrain_output_fetcher_->right_high();
if (left_gear_high_) {
if (right_gear_high_) {
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.h b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
index 9a4f177..2284f63 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.h
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
@@ -1,11 +1,15 @@
#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_TEST_LIB_H_
#define FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_TEST_LIB_H_
-#include "aos/events/event-loop.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "aos/events/event_loop.h"
+#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/state_feedback_loop.h"
-#include "frc971/queues/gyro.q.h"
+#include "frc971/queues/gyro_generated.h"
namespace frc971 {
namespace control_loops {
@@ -79,11 +83,11 @@
::aos::EventLoop *event_loop_;
::aos::Fetcher<::aos::RobotState> robot_state_fetcher_;
- ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Position>
+ ::aos::Sender<::frc971::control_loops::drivetrain::Position>
drivetrain_position_sender_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Output>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Output>
drivetrain_output_fetcher_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
::aos::Sender<::frc971::sensors::GyroReading> gyro_reading_sender_;
@@ -95,7 +99,8 @@
::Eigen::Matrix<double, 5, 1> state_ = ::Eigen::Matrix<double, 5, 1>::Zero();
::std::unique_ptr<
StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
- HybridKalman<2, 2, 2>>> velocity_drivetrain_;
+ HybridKalman<2, 2, 2>>>
+ velocity_drivetrain_;
double last_left_position_;
double last_right_position_;
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
index 8234b1c..7fe5cad 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
@@ -105,9 +105,9 @@
void LineFollowDrivetrain::SetGoal(
::aos::monotonic_clock::time_point now,
- const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
+ const ::frc971::control_loops::drivetrain::Goal *goal) {
// TODO(james): More properly calculate goal velocity from throttle.
- goal_velocity_ = 4.0 * goal.throttle;
+ goal_velocity_ = 4.0 * goal->throttle();
// The amount of time to freeze the target choice after the driver releases
// the button. Depending on the current state, we vary how long this timeout
// is so that we can avoid button glitches causing issues.
@@ -115,7 +115,7 @@
// Freeze the target once the driver presses the button; if we haven't yet
// confirmed a target when the driver presses the button, we will not do
// anything and report not ready until we have a target.
- if (goal.controller_type == 3) {
+ if (goal->controller_type() == drivetrain::ControllerType_LINE_FOLLOWER) {
last_enable_ = now;
// If we already acquired a target, we want to keep track if it.
if (have_target_) {
@@ -130,11 +130,11 @@
freeze_target_ = now <= freeze_delay + last_enable_;
// Set an adjustment that lets the driver tweak the offset for where we place
// the target left/right.
- side_adjust_ = -goal.wheel * 0.1;
+ side_adjust_ = -goal->wheel() * 0.1;
}
bool LineFollowDrivetrain::SetOutput(
- ::frc971::control_loops::DrivetrainQueue::Output *output) {
+ ::frc971::control_loops::drivetrain::OutputT *output) {
// TODO(james): Account for voltage error terms, and/or provide driver with
// ability to influence steering.
if (output != nullptr && have_target_) {
@@ -257,18 +257,20 @@
}
}
-void LineFollowDrivetrain::PopulateStatus(
- ::frc971::control_loops::DrivetrainQueue::Status *status) const {
- status->line_follow_logging.frozen = freeze_target_;
- status->line_follow_logging.have_target = have_target_;
- status->line_follow_logging.x = target_pose_.abs_pos().x();
- status->line_follow_logging.y = target_pose_.abs_pos().y();
- status->line_follow_logging.theta = target_pose_.abs_theta();
- status->line_follow_logging.offset = relative_pose_.rel_pos().y();
- status->line_follow_logging.distance_to_target =
- -relative_pose_.rel_pos().x();
- status->line_follow_logging.goal_theta = controls_goal_(0, 0);
- status->line_follow_logging.rel_theta = relative_pose_.rel_theta();
+flatbuffers::Offset<LineFollowLogging> LineFollowDrivetrain::PopulateStatus(
+ aos::Sender<drivetrain::Status>::Builder *builder) const {
+ LineFollowLogging::Builder line_follow_logging_builder =
+ builder->MakeBuilder<LineFollowLogging>();
+ line_follow_logging_builder.add_frozen(freeze_target_);
+ line_follow_logging_builder.add_have_target(have_target_);
+ line_follow_logging_builder.add_x(target_pose_.abs_pos().x());
+ line_follow_logging_builder.add_y(target_pose_.abs_pos().y());
+ line_follow_logging_builder.add_theta(target_pose_.abs_theta());
+ line_follow_logging_builder.add_offset(relative_pose_.rel_pos().y());
+ line_follow_logging_builder.add_distance_to_target(-relative_pose_.rel_pos().x());
+ line_follow_logging_builder.add_goal_theta(controls_goal_(0, 0));
+ line_follow_logging_builder.add_rel_theta(relative_pose_.rel_theta());
+ return line_follow_logging_builder.Finish();
}
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.h b/frc971/control_loops/drivetrain/line_follow_drivetrain.h
index 09b2080..1ebf64d 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.h
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.h
@@ -2,10 +2,15 @@
#define FRC971_CONTROL_LOOPS_DRIVETRAIN_LINE_FOLLOW_DRIVETRAIN_H_
#include "Eigen/Dense"
-#include "frc971/control_loops/pose.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/drivetrain/localizer.h"
+#include "frc971/control_loops/pose.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
namespace frc971 {
namespace control_loops {
@@ -29,7 +34,7 @@
// a Target; the positive X-axis in the Pose's frame represents the direction
// we want to go (we approach the pose from the left-half plane).
void SetGoal(::aos::monotonic_clock::time_point now,
- const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
+ const ::frc971::control_loops::drivetrain::Goal *goal);
// State: [x, y, theta, left_vel, right_vel]
void Update(::aos::monotonic_clock::time_point now,
const ::Eigen::Matrix<double, 5, 1> &state);
@@ -37,9 +42,11 @@
// yet have a target to track into and so some other drivetrain should take
// over.
bool SetOutput(
- ::frc971::control_loops::DrivetrainQueue::Output *output);
- void PopulateStatus(
- ::frc971::control_loops::DrivetrainQueue::Status *status) const;
+ ::frc971::control_loops::drivetrain::OutputT *output);
+
+ flatbuffers::Offset<LineFollowLogging> PopulateStatus(
+ aos::Sender<drivetrain::Status>::Builder *line_follow_logging_builder)
+ const;
private:
// Nominal max voltage.
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
index f4d718e..bd45e92 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
@@ -50,13 +50,21 @@
}
void Iterate() {
- ::frc971::control_loops::DrivetrainQueue::Goal goal;
- goal.throttle = driver_model_(state_);
- goal.controller_type = freeze_target_ ? 3 : 0;
- drivetrain_.SetGoal(t_, goal);
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.ForceDefaults(1);
+ Goal::Builder goal_builder(fbb);
+ goal_builder.add_throttle(driver_model_(state_));
+ goal_builder.add_controller_type(freeze_target_
+ ? ControllerType_LINE_FOLLOWER
+ : ControllerType_POLYDRIVE);
+ fbb.Finish(goal_builder.Finish());
+
+ aos::FlatbufferDetachedBuffer<Goal> goal(fbb.Release());
+
+ drivetrain_.SetGoal(t_, &goal.message());
drivetrain_.Update(t_, state_);
- ::frc971::control_loops::DrivetrainQueue::Output output;
+ ::frc971::control_loops::drivetrain::OutputT output;
EXPECT_EQ(expect_output_, drivetrain_.SetOutput(&output));
if (!expect_output_) {
EXPECT_EQ(0.0, output.left_voltage);
@@ -96,9 +104,15 @@
}
double GoalTheta(double x, double y, double v, double throttle) {
- ::frc971::control_loops::DrivetrainQueue::Goal goal;
- goal.throttle = throttle;
- drivetrain_.SetGoal(t_, goal);
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.ForceDefaults(1);
+ Goal::Builder goal_builder(fbb);
+ goal_builder.add_throttle(throttle);
+ fbb.Finish(goal_builder.Finish());
+
+ aos::FlatbufferDetachedBuffer<Goal> goal(fbb.Release());
+
+ drivetrain_.SetGoal(t_, &goal.message());
::Eigen::Matrix<double, 5, 1> state;
state << x, y, 0.0, v, v;
return drivetrain_.GoalTheta(state, y, throttle > 0.0 ? 1.0 : -1.0);
diff --git a/frc971/control_loops/drivetrain/localizer.fbs b/frc971/control_loops/drivetrain/localizer.fbs
new file mode 100644
index 0000000..5450c08
--- /dev/null
+++ b/frc971/control_loops/drivetrain/localizer.fbs
@@ -0,0 +1,13 @@
+namespace frc971.control_loops.drivetrain;
+
+// Allows you to reset the state of the localizer to a specific position on the
+// field.
+table LocalizerControl {
+ x:float; // X position, meters
+ y:float; // Y position, meters
+ theta:float; // heading, radians
+ theta_uncertainty:double; // Uncertainty in theta.
+ keep_current_theta:bool; // Whether to keep the current theta value.
+}
+
+root_type LocalizerControl;
diff --git a/frc971/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localizer.h
index 7b2ceae..2589d4b 100644
--- a/frc971/control_loops/drivetrain/localizer.h
+++ b/frc971/control_loops/drivetrain/localizer.h
@@ -1,7 +1,7 @@
#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_FIELD_ESTIMATOR_H_
#define FRC971_CONTROL_LOOPS_DRIVETRAIN_FIELD_ESTIMATOR_H_
-#include "aos/events/event-loop.h"
+#include "aos/events/event_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/drivetrain/hybrid_ekf.h"
#include "frc971/control_loops/pose.h"
diff --git a/frc971/control_loops/drivetrain/localizer.q b/frc971/control_loops/drivetrain/localizer.q
deleted file mode 100644
index 323f895..0000000
--- a/frc971/control_loops/drivetrain/localizer.q
+++ /dev/null
@@ -1,12 +0,0 @@
-package frc971.control_loops.drivetrain;
-
-// Allows you to reset the state of the localizer to a specific position on the
-// field.
-// Published on ".frc971.control_loops.drivetrain.localizer_control"
-message LocalizerControl {
- float x; // X position, meters
- float y; // Y position, meters
- float theta; // heading, radians
- double theta_uncertainty; // Uncertainty in theta.
- bool keep_current_theta; // Whether to keep the current theta value.
-};
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.cc b/frc971/control_loops/drivetrain/polydrivetrain.cc
index c213b06..a0d374e 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain.cc
@@ -1,14 +1,5 @@
#include "frc971/control_loops/drivetrain/polydrivetrain.h"
-#include "aos/commonmath.h"
-#include "aos/controls/polytope.h"
-#include "frc971/control_loops/coerce_goal.h"
-#ifdef __linux__
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/control_loops/drivetrain/drivetrain_config.h"
-#endif // __linux__
-#include "frc971/control_loops/state_feedback_loop.h"
-
namespace frc971 {
namespace control_loops {
namespace drivetrain {
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.h b/frc971/control_loops/drivetrain/polydrivetrain.h
index f8f1a39..74fb2eb 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.h
+++ b/frc971/control_loops/drivetrain/polydrivetrain.h
@@ -7,13 +7,18 @@
#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/drivetrain/gear.h"
#ifdef __linux__
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "aos/logging/logging.h"
-#include "aos/logging/matrix_logging.h"
-#include "aos/logging/queue_logging.h"
-#include "aos/robot_state/robot_state.q.h"
+#include "aos/robot_state/robot_state_generated.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#else
-#include "frc971/control_loops/drivetrain/drivetrain_uc.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_float_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_float_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_float_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_float_generated.h"
#endif // __linux__
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
@@ -35,10 +40,11 @@
Scalar MotorSpeed(const constants::ShifterHallEffect &hall_effect,
Scalar shifter_position, Scalar velocity, Gear gear);
- void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
+ void SetGoal(const Scalar wheel, const Scalar throttle, const bool quickturn,
+ const bool highgear);
void SetPosition(
- const ::frc971::control_loops::DrivetrainQueue::Position *position,
+ const ::frc971::control_loops::drivetrain::Position *position,
Gear left_gear, Gear right_gear);
Scalar FilterVelocity(Scalar throttle) const;
@@ -47,9 +53,10 @@
void Update(Scalar voltage_battery);
- void SetOutput(::frc971::control_loops::DrivetrainQueue::Output *output);
+ void SetOutput(::frc971::control_loops::drivetrain::OutputT *output);
- void PopulateStatus(::frc971::control_loops::DrivetrainQueue::Status *status);
+ flatbuffers::Offset<CIMLogging> PopulateStatus(
+ flatbuffers::FlatBufferBuilder *fbb);
// Computes the next state of a shifter given the current state and the
// requested state.
@@ -81,8 +88,8 @@
Gear left_gear_;
Gear right_gear_;
- ::frc971::control_loops::DrivetrainQueue::Position last_position_;
- ::frc971::control_loops::DrivetrainQueue::Position position_;
+ ::frc971::control_loops::drivetrain::PositionT last_position_;
+ ::frc971::control_loops::drivetrain::PositionT position_;
int counter_;
DrivetrainConfig<Scalar> dt_config_;
@@ -123,10 +130,7 @@
left_gear_(dt_config.default_high_gear ? Gear::HIGH : Gear::LOW),
right_gear_(dt_config.default_high_gear ? Gear::HIGH : Gear::LOW),
counter_(0),
- dt_config_(dt_config) {
- last_position_.Zero();
- position_.Zero();
-}
+ dt_config_(dt_config) {}
template <typename Scalar>
Scalar PolyDrivetrain<Scalar>::MotorSpeed(
@@ -195,13 +199,9 @@
}
template <typename Scalar>
-void PolyDrivetrain<Scalar>::SetGoal(
- const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
- const Scalar wheel = goal.wheel;
- const Scalar throttle = goal.throttle;
- const bool quickturn = goal.quickturn;
- const bool highgear = goal.highgear;
-
+void PolyDrivetrain<Scalar>::SetGoal(const Scalar wheel, const Scalar throttle,
+ const bool quickturn,
+ const bool highgear) {
// Apply a sin function that's scaled to make it feel better.
const Scalar angular_range =
static_cast<Scalar>(M_PI_2) * dt_config_.wheel_non_linearity;
@@ -234,12 +234,12 @@
template <typename Scalar>
void PolyDrivetrain<Scalar>::SetPosition(
- const ::frc971::control_loops::DrivetrainQueue::Position *position,
+ const ::frc971::control_loops::drivetrain::Position *position,
Gear left_gear, Gear right_gear) {
left_gear_ = left_gear;
right_gear_ = right_gear;
last_position_ = position_;
- position_ = *position;
+ position->UnPackTo(&position_);
}
template <typename Scalar>
@@ -415,8 +415,8 @@
template <typename Scalar>
void PolyDrivetrain<Scalar>::SetOutput(
- ::frc971::control_loops::DrivetrainQueue::Output *output) {
- if (output != NULL) {
+ ::frc971::control_loops::drivetrain::OutputT *output) {
+ if (output != nullptr) {
output->left_voltage = loop_->U(0, 0);
output->right_voltage = loop_->U(1, 0);
output->left_high = MaybeHigh(left_gear_);
@@ -425,19 +425,21 @@
}
template <typename Scalar>
-void PolyDrivetrain<Scalar>::PopulateStatus(
- ::frc971::control_loops::DrivetrainQueue::Status *status) {
+flatbuffers::Offset<CIMLogging> PolyDrivetrain<Scalar>::PopulateStatus(
+ flatbuffers::FlatBufferBuilder *fbb) {
+ CIMLogging::Builder builder(*fbb);
- status->cim_logging.left_in_gear = IsInGear(left_gear_);
- status->cim_logging.left_motor_speed = left_motor_speed_;
- status->cim_logging.left_velocity = current_left_velocity_;
+ builder.add_left_in_gear(IsInGear(left_gear_));
+ builder.add_left_motor_speed(left_motor_speed_);
+ builder.add_left_velocity(current_left_velocity_);
- status->cim_logging.right_in_gear = IsInGear(right_gear_);
- status->cim_logging.right_motor_speed = right_motor_speed_;
- status->cim_logging.right_velocity = current_right_velocity_;
+ builder.add_right_in_gear(IsInGear(right_gear_));
+ builder.add_right_motor_speed(right_motor_speed_);
+ builder.add_right_velocity(current_right_velocity_);
+
+ return builder.Finish();
}
-
} // namespace drivetrain
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/replay_drivetrain.cc b/frc971/control_loops/drivetrain/replay_drivetrain.cc
deleted file mode 100644
index 2f63c9e..0000000
--- a/frc971/control_loops/drivetrain/replay_drivetrain.cc
+++ /dev/null
@@ -1,38 +0,0 @@
-#include "aos/controls/replay_control_loop.h"
-#include "aos/init.h"
-
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/queues/gyro.q.h"
-
-// Reads one or more log files and sends out all the queue messages (in the
-// correct order and at the correct time) to feed a "live" drivetrain process.
-
-int main(int argc, char **argv) {
- if (argc <= 1) {
- fprintf(stderr, "Need at least one file to replay!\n");
- return EXIT_FAILURE;
- }
-
- ::aos::InitNRT();
-
- ::frc971::control_loops::DrivetrainQueue drivetrain_queue(
- ".frc971.control_loops.drivetrain_queue",
- ".frc971.control_loops.drivetrain_queue.goal",
- ".frc971.control_loops.drivetrain_queue.position",
- ".frc971.control_loops.drivetrain_queue.output",
- ".frc971.control_loops.drivetrain_queue.status");
-
- {
- ::aos::controls::ControlLoopReplayer<
- ::frc971::control_loops::DrivetrainQueue>
- replayer(&drivetrain_queue, "drivetrain");
-
- replayer.AddDirectQueueSender<::frc971::sensors::GyroReading>(
- "wpilib_interface.Gyro", "sending", ".frc971.sensors.gyro_reading");
- for (int i = 1; i < argc; ++i) {
- replayer.ProcessFile(argv[i]);
- }
- }
-
- ::aos::Cleanup();
-}
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index de59355..1222ae0 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -2,10 +2,13 @@
#include "Eigen/Dense"
-#include "aos/init.h"
+#include "aos/realtime.h"
#include "aos/util/math.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
namespace frc971 {
namespace control_loops {
@@ -30,7 +33,7 @@
::aos::MutexLocker locker(&mutex_);
while (run_) {
- while (goal_.spline.spline_idx == future_spline_idx_) {
+ while (goal_.spline_idx == future_spline_idx_) {
AOS_CHECK(!new_goal_.Wait());
if (!run_) {
return;
@@ -39,10 +42,10 @@
past_distance_spline_.reset();
past_trajectory_.reset();
- plan_state_ = PlanState::kBuildingTrajectory;
- const ::frc971::MultiSpline &multispline = goal_.spline;
- future_spline_idx_ = multispline.spline_idx;
+ plan_state_ = PlanningState_BUILDING_TRAJECTORY;
+ future_spline_idx_ = goal_.spline_idx;
planning_spline_idx_ = future_spline_idx_;
+ const MultiSpline &multispline = goal_.spline;
auto x = multispline.spline_x;
auto y = multispline.spline_y;
::std::vector<Spline> splines = ::std::vector<Spline>();
@@ -56,7 +59,7 @@
splines.emplace_back(Spline(points));
}
- future_drive_spline_backwards_ = goal_.spline.drive_spline_backwards;
+ future_drive_spline_backwards_ = goal_.drive_spline_backwards;
future_distance_spline_ = ::std::unique_ptr<DistanceSpline>(
new DistanceSpline(::std::move(splines)));
@@ -65,47 +68,79 @@
new Trajectory(future_distance_spline_.get(), dt_config_));
for (size_t i = 0; i < multispline.constraints.size(); ++i) {
- const ::frc971::Constraint &constraint = multispline.constraints[i];
+ const ::frc971::ConstraintT &constraint = multispline.constraints[i];
switch (constraint.constraint_type) {
- case 0:
+ case frc971::ConstraintType_CONSTRAINT_TYPE_UNDEFINED:
break;
- case 1:
+ case frc971::ConstraintType_LONGITUDINAL_ACCELERATION:
future_trajectory_->set_longitudal_acceleration(constraint.value);
break;
- case 2:
+ case frc971::ConstraintType_LATERAL_ACCELERATION:
future_trajectory_->set_lateral_acceleration(constraint.value);
break;
- case 3:
+ case frc971::ConstraintType_VOLTAGE:
future_trajectory_->set_voltage_limit(constraint.value);
break;
- case 4:
+ case frc971::ConstraintType_VELOCITY:
future_trajectory_->LimitVelocity(constraint.start_distance,
constraint.end_distance,
constraint.value);
break;
}
}
- plan_state_ = PlanState::kPlanningTrajectory;
+ plan_state_ = PlanningState_PLANNING_TRAJECTORY;
future_trajectory_->Plan();
- plan_state_ = PlanState::kPlannedTrajectory;
+ plan_state_ = PlanningState_PLANNED;
}
}
void SplineDrivetrain::SetGoal(
- const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
- current_spline_handle_ = goal.spline_handle;
+ const ::frc971::control_loops::drivetrain::Goal *goal) {
+ current_spline_handle_ = goal->spline_handle();
+
// If told to stop, set the executing spline to an invalid index and clear out
// its plan:
if (current_spline_handle_ == 0 &&
- current_spline_idx_ != goal.spline.spline_idx) {
+ (goal->spline() == nullptr ||
+ current_spline_idx_ != CHECK_NOTNULL(goal->spline())->spline_idx())) {
current_spline_idx_ = -1;
}
::aos::Mutex::State mutex_state = mutex_.TryLock();
if (mutex_state == ::aos::Mutex::State::kLocked) {
- if (goal.spline.spline_idx && future_spline_idx_ != goal.spline.spline_idx) {
- goal_ = goal;
+ if (goal->spline() != nullptr && goal->spline()->spline_idx() &&
+ future_spline_idx_ != goal->spline()->spline_idx()) {
+ CHECK_NOTNULL(goal->spline());
+ goal_.spline_idx = goal->spline()->spline_idx();
+ goal_.drive_spline_backwards = goal->spline()->drive_spline_backwards();
+
+ const frc971::MultiSpline *multispline = goal->spline()->spline();
+ CHECK_NOTNULL(multispline);
+
+ goal_.spline.spline_count = multispline->spline_count();
+
+ CHECK_EQ(multispline->spline_x()->size(),
+ static_cast<size_t>(goal_.spline.spline_count * 5 + 1));
+ CHECK_EQ(multispline->spline_y()->size(),
+ static_cast<size_t>(goal_.spline.spline_count * 5 + 1));
+
+ std::copy(multispline->spline_x()->begin(),
+ multispline->spline_x()->end(), goal_.spline.spline_x.begin());
+ std::copy(multispline->spline_y()->begin(),
+ multispline->spline_y()->end(), goal_.spline.spline_y.begin());
+
+ for (size_t i = 0; i < 6; ++i) {
+ if (multispline->constraints() != nullptr &&
+ i < multispline->constraints()->size()) {
+ multispline->constraints()->Get(i)->UnPackTo(
+ &goal_.spline.constraints[i]);
+ } else {
+ goal_.spline.constraints[i].constraint_type =
+ ConstraintType_CONSTRAINT_TYPE_UNDEFINED;
+ }
+ }
+
new_goal_.Broadcast();
if (current_spline_handle_ != current_spline_idx_) {
// If we aren't going to actively execute the current spline, evict it's
@@ -178,7 +213,7 @@
}
void SplineDrivetrain::SetOutput(
- ::frc971::control_loops::DrivetrainQueue::Output *output) {
+ ::frc971::control_loops::drivetrain::OutputT *output) {
if (!output) {
return;
}
@@ -193,42 +228,52 @@
output->right_voltage = next_U_(1);
}
+
void SplineDrivetrain::PopulateStatus(
- ::frc971::control_loops::DrivetrainQueue::Status *status) const {
- if (status && enable_) {
- status->uncapped_left_voltage = uncapped_U_(0);
- status->uncapped_right_voltage = uncapped_U_(1);
- status->robot_speed = current_xva_(1);
- status->output_was_capped = output_was_capped_;
+ drivetrain::Status::Builder *builder) const {
+ if (builder && enable_) {
+ builder->add_uncapped_left_voltage(uncapped_U_(0));
+ builder->add_uncapped_right_voltage(uncapped_U_(1));
+ builder->add_robot_speed(current_xva_(1));
+ builder->add_output_was_capped(output_was_capped_);
}
+}
- if (status) {
- if (current_distance_spline_) {
- ::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
- status->trajectory_logging.x = goal_state(0);
- status->trajectory_logging.y = goal_state(1);
- status->trajectory_logging.theta = ::aos::math::NormalizeAngle(
- goal_state(2) + (current_drive_spline_backwards_ ? M_PI : 0.0));
- status->trajectory_logging.left_velocity = goal_state(3);
- status->trajectory_logging.right_velocity = goal_state(4);
- }
- status->trajectory_logging.planning_state = static_cast<int8_t>(plan_state_.load());
- status->trajectory_logging.is_executing = !IsAtEnd() && has_started_execution_;
- status->trajectory_logging.is_executed =
- (current_spline_idx_ != -1) && IsAtEnd();
- status->trajectory_logging.goal_spline_handle = current_spline_handle_;
- status->trajectory_logging.current_spline_idx = current_spline_idx_;
- status->trajectory_logging.distance_remaining =
- current_trajectory_ ? current_trajectory_->length() - current_xva_.x()
- : 0.0;
-
- int32_t planning_spline_idx = planning_spline_idx_;
- if (current_spline_idx_ == planning_spline_idx) {
- status->trajectory_logging.planning_spline_idx = 0;
- } else {
- status->trajectory_logging.planning_spline_idx = planning_spline_idx_;
- }
+flatbuffers::Offset<TrajectoryLogging> SplineDrivetrain::MakeTrajectoryLogging(
+ flatbuffers::FlatBufferBuilder *builder) const {
+ drivetrain::TrajectoryLogging::Builder trajectory_logging_builder(*builder);
+ if (current_distance_spline_) {
+ ::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
+ trajectory_logging_builder.add_x(goal_state(0));
+ trajectory_logging_builder.add_y(goal_state(1));
+ trajectory_logging_builder.add_theta(::aos::math::NormalizeAngle(
+ goal_state(2) + (current_drive_spline_backwards_ ? M_PI : 0.0)));
+ trajectory_logging_builder.add_left_velocity(goal_state(3));
+ trajectory_logging_builder.add_right_velocity(goal_state(4));
}
+ trajectory_logging_builder.add_planning_state(plan_state_.load());
+ trajectory_logging_builder.add_is_executing(!IsAtEnd() &&
+ has_started_execution_);
+ trajectory_logging_builder.add_is_executed((current_spline_idx_ != -1) &&
+ IsAtEnd());
+ trajectory_logging_builder.add_goal_spline_handle(current_spline_handle_);
+ trajectory_logging_builder.add_current_spline_idx(current_spline_idx_);
+ trajectory_logging_builder.add_distance_remaining(
+ current_trajectory_ ? current_trajectory_->length() - current_xva_.x()
+ : 0.0);
+
+ int32_t planning_spline_idx = planning_spline_idx_;
+ if (current_spline_idx_ == planning_spline_idx) {
+ trajectory_logging_builder.add_planning_spline_idx(0);
+ } else {
+ trajectory_logging_builder.add_planning_spline_idx(planning_spline_idx_);
+ }
+ return trajectory_logging_builder.Finish();
+}
+
+flatbuffers::Offset<TrajectoryLogging> SplineDrivetrain::MakeTrajectoryLogging(
+ aos::Sender<drivetrain::Status>::Builder *builder) const {
+ return MakeTrajectoryLogging(builder->fbb());
}
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index 98c4e74..4f21b29 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -8,9 +8,12 @@
#include "aos/condition.h"
#include "aos/mutex/mutex.h"
+#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/distance_spline.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/drivetrain/spline.h"
#include "frc971/control_loops/drivetrain/trajectory.h"
@@ -31,14 +34,18 @@
worker_thread_.join();
}
- void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
+ void SetGoal(const ::frc971::control_loops::drivetrain::Goal *goal);
void Update(bool enabled, const ::Eigen::Matrix<double, 5, 1> &state);
- void SetOutput(
- ::frc971::control_loops::DrivetrainQueue::Output *output);
+ void SetOutput(::frc971::control_loops::drivetrain::OutputT *output);
+
+ flatbuffers::Offset<TrajectoryLogging> MakeTrajectoryLogging(
+ aos::Sender<drivetrain::Status>::Builder *builder) const;
+ flatbuffers::Offset<TrajectoryLogging> MakeTrajectoryLogging(
+ flatbuffers::FlatBufferBuilder *builder) const;
void PopulateStatus(
- ::frc971::control_loops::DrivetrainQueue::Status *status) const;
+ drivetrain::Status::Builder *status) const;
// Accessor for the current goal state, pretty much only present for debugging
// purposes.
@@ -55,6 +62,9 @@
true;
}
+ // Returns true if the splinedrivetrain is enabled.
+ bool enable() const { return enable_; }
+
enum class PlanState : int8_t {
kNoPlan = 0,
kBuildingTrajectory = 1,
@@ -85,7 +95,7 @@
bool enable_ = false;
bool output_was_capped_ = false;
- std::atomic<PlanState> plan_state_ = {PlanState::kNoPlan};
+ std::atomic<PlanningState> plan_state_ = {PlanningState_NO_PLAN};
::std::thread worker_thread_;
// mutex_ is held by the worker thread while it is doing work or by the main
@@ -95,7 +105,26 @@
::aos::Condition new_goal_;
// The following variables are guarded by mutex_.
bool run_ = true;
- ::frc971::control_loops::DrivetrainQueue::Goal goal_;
+
+ // These two structures mirror the flatbuffer Multispline.
+ // TODO(austin): copy the goal flatbuffer directly instead of recreating it
+ // like this...
+ struct MultiSpline {
+ int32_t spline_count;
+ std::array<float, 36> spline_x;
+ std::array<float, 36> spline_y;
+ std::array<ConstraintT, 6> constraints;
+ };
+
+ struct SplineGoal {
+ int32_t spline_idx = 0;
+
+ bool drive_spline_backwards;
+
+ MultiSpline spline;
+ };
+
+ SplineGoal goal_;
::std::unique_ptr<DistanceSpline> past_distance_spline_;
::std::unique_ptr<DistanceSpline> future_distance_spline_;
::std::unique_ptr<Trajectory> past_trajectory_;
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index 5924eb1..cd29e39 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -2,11 +2,12 @@
#include "aos/commonmath.h"
#include "aos/controls/polytope.h"
-#include "aos/logging/matrix_logging.h"
#include "frc971/control_loops/coerce_goal.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/state_feedback_loop.h"
namespace frc971 {
@@ -60,8 +61,6 @@
const Eigen::Matrix<double, 7, 1> error = kf_->R() - kf_->X_hat();
- AOS_LOG_MATRIX(DEBUG, "U_uncapped", *U);
-
Eigen::Matrix<double, 2, 2> position_K;
position_K << kf_->controller().K(0, 0), kf_->controller().K(0, 2),
kf_->controller().K(1, 0), kf_->controller().K(1, 2);
@@ -75,7 +74,6 @@
const auto drive_error = T_inverse_ * position_error;
Eigen::Matrix<double, 2, 1> velocity_error;
velocity_error << error(1, 0), error(3, 0);
- AOS_LOG_MATRIX(DEBUG, "error", error);
Eigen::Matrix<double, 2, 1> U_integral;
U_integral << kf_->X_hat(4, 0), kf_->X_hat(5, 0);
@@ -144,23 +142,31 @@
}
void DrivetrainMotorsSS::SetGoal(
- const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
- unprofiled_goal_ << goal.left_goal, 0.0, goal.right_goal, 0.0, 0.0, 0.0, 0.0;
- if (::std::abs(goal.max_ss_voltage) < 0.01) {
+ const ::frc971::control_loops::drivetrain::Goal *goal) {
+ unprofiled_goal_ << goal->left_goal(), 0.0, goal->right_goal(), 0.0, 0.0, 0.0,
+ 0.0;
+ if (!goal->has_max_ss_voltage()) {
max_voltage_ = kMaxVoltage;
} else {
- max_voltage_ = goal.max_ss_voltage;
+ max_voltage_ = goal->has_max_ss_voltage();
}
- use_profile_ =
- !kf_->controller().Kff().isZero(0) &&
- (goal.linear.max_velocity != 0.0 && goal.linear.max_acceleration != 0.0 &&
- goal.angular.max_velocity != 0.0 &&
- goal.angular.max_acceleration != 0.0);
- linear_profile_.set_maximum_velocity(goal.linear.max_velocity);
- linear_profile_.set_maximum_acceleration(goal.linear.max_acceleration);
- angular_profile_.set_maximum_velocity(goal.angular.max_velocity);
- angular_profile_.set_maximum_acceleration(goal.angular.max_acceleration);
+ use_profile_ = !kf_->controller().Kff().isZero(0) &&
+ (goal->has_linear() && goal->has_angular() &&
+ goal->linear()->has_max_velocity() &&
+ goal->linear()->has_max_acceleration() &&
+ goal->angular()->has_max_velocity() &&
+ goal->angular()->has_max_acceleration());
+ if (goal->has_linear()) {
+ linear_profile_.set_maximum_velocity(goal->linear()->max_velocity());
+ linear_profile_.set_maximum_acceleration(
+ goal->linear()->max_acceleration());
+ }
+ if (goal->has_angular()) {
+ angular_profile_.set_maximum_velocity(goal->angular()->max_velocity());
+ angular_profile_.set_maximum_acceleration(
+ goal->angular()->max_acceleration());
+ }
}
void DrivetrainMotorsSS::Update(bool enable_control_loop) {
@@ -255,7 +261,7 @@
}
void DrivetrainMotorsSS::SetOutput(
- ::frc971::control_loops::DrivetrainQueue::Output *output) const {
+ ::frc971::control_loops::drivetrain::OutputT *output) const {
if (output) {
output->left_voltage = kf_->U(0, 0);
output->right_voltage = kf_->U(1, 0);
@@ -265,7 +271,7 @@
}
void DrivetrainMotorsSS::PopulateStatus(
- ::frc971::control_loops::DrivetrainQueue::Status *status) const {
+ ::frc971::control_loops::drivetrain::StatusBuilder *builder) const {
Eigen::Matrix<double, 2, 1> profiled_linear =
dt_config_.LeftRightToLinear(kf_->next_R());
Eigen::Matrix<double, 2, 1> profiled_angular =
@@ -276,10 +282,10 @@
Eigen::Matrix<double, 4, 1> profiled_gyro_left_right =
dt_config_.AngularLinearToLeftRight(profiled_linear, profiled_angular);
- status->profiled_left_position_goal = profiled_gyro_left_right(0, 0);
- status->profiled_left_velocity_goal = profiled_gyro_left_right(1, 0);
- status->profiled_right_position_goal = profiled_gyro_left_right(2, 0);
- status->profiled_right_velocity_goal = profiled_gyro_left_right(3, 0);
+ builder->add_profiled_left_position_goal(profiled_gyro_left_right(0, 0));
+ builder->add_profiled_left_velocity_goal(profiled_gyro_left_right(1, 0));
+ builder->add_profiled_right_position_goal(profiled_gyro_left_right(2, 0));
+ builder->add_profiled_right_velocity_goal(profiled_gyro_left_right(3, 0));
}
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.h b/frc971/control_loops/drivetrain/ssdrivetrain.h
index 762cbc2..4d91807 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.h
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.h
@@ -4,14 +4,16 @@
#include "aos/commonmath.h"
#include "aos/controls/control_loop.h"
#include "aos/controls/polytope.h"
-#include "aos/logging/matrix_logging.h"
#include "aos/util/trapezoid_profile.h"
-#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/coerce_goal.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/drivetrain/localizer.h"
+#include "frc971/control_loops/state_feedback_loop.h"
namespace frc971 {
namespace control_loops {
@@ -23,7 +25,7 @@
StateFeedbackLoop<7, 2, 4> *kf,
LocalizerInterface *localizer);
- void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
+ void SetGoal(const ::frc971::control_loops::drivetrain::Goal *goal);
// Computes the power to send out as part of the controller. Should be called
// when disabled (with enable_control_loop false) so the profiles get computed
@@ -34,10 +36,9 @@
bool output_was_capped() const { return output_was_capped_; }
- void SetOutput(
- ::frc971::control_loops::DrivetrainQueue::Output *output) const;
+ void SetOutput(::frc971::control_loops::drivetrain::OutputT *output) const;
void PopulateStatus(
- ::frc971::control_loops::DrivetrainQueue::Status *status) const;
+ ::frc971::control_loops::drivetrain::StatusBuilder *builder) const;
private:
void PolyCapU(Eigen::Matrix<double, 2, 1> *U);
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index 9d35a5e..ba4c0c2 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -3,7 +3,6 @@
#include <chrono>
#include "Eigen/Dense"
-#include "aos/logging/matrix_logging.h"
#include "frc971/control_loops/c2d.h"
#include "frc971/control_loops/dlqr.h"
#include "frc971/control_loops/drivetrain/distance_spline.h"
@@ -327,9 +326,7 @@
::Eigen::Matrix<double, 2, 5> K = ::Eigen::Matrix<double, 2, 5>::Zero();
int info = ::frc971::controls::dlqr<5, 2>(A, B, Q, R, &K, &S);
- if (info == 0) {
- AOS_LOG_MATRIX(INFO, "K", K);
- } else {
+ if (info != 0) {
AOS_LOG(ERROR, "Failed to solve %d, controllability: %d\n", info,
controls::Controllability(A, B));
// TODO(austin): Can we be more clever here? Use the last one? We should
diff --git a/frc971/control_loops/drivetrain/trajectory_plot.cc b/frc971/control_loops/drivetrain/trajectory_plot.cc
index 0b35ee8..2a990f9 100644
--- a/frc971/control_loops/drivetrain/trajectory_plot.cc
+++ b/frc971/control_loops/drivetrain/trajectory_plot.cc
@@ -3,7 +3,6 @@
#include <chrono>
#include "aos/logging/implementations.h"
-#include "aos/logging/matrix_logging.h"
#include "aos/network/team_number.h"
#include "aos/time/time.h"
#include "frc971/control_loops/dlqr.h"