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/BUILD b/frc971/control_loops/BUILD
index 3d6c930..17613ab 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -1,6 +1,6 @@
package(default_visibility = ["//visibility:public"])
-load("//aos/build:queues.bzl", "queue_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
load("//tools:environments.bzl", "mcu_cpus")
cc_library(
@@ -34,7 +34,7 @@
hdrs = ["pose.h"],
deps = [
"//aos/util:math",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -53,15 +53,16 @@
"hall_effect_tracker.h",
],
deps = [
- ":queues",
+ ":control_loops_fbs",
],
)
-queue_library(
- name = "queues",
+flatbuffer_cc_library(
+ name = "control_loops_fbs",
srcs = [
- "control_loops.q",
+ "control_loops.fbs",
],
+ compatible_with = mcu_cpus,
)
cc_test(
@@ -70,8 +71,8 @@
"position_sensor_sim_test.cc",
],
deps = [
+ ":control_loops_fbs",
":position_sensor_sim",
- ":queues",
"//aos/logging",
"//aos/testing:googletest",
],
@@ -90,8 +91,8 @@
"-lm",
],
deps = [
+ ":control_loops_fbs",
":gaussian_noise",
- ":queues",
"//aos/testing:random_seed",
],
)
@@ -120,7 +121,7 @@
restricted_to = mcu_cpus,
deps = [
"//aos/controls:polytope_uc",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -137,7 +138,7 @@
],
deps = [
"//aos/controls:polytope",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -151,7 +152,7 @@
restricted_to = mcu_cpus,
deps = [
"//aos:macros",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -163,7 +164,7 @@
deps = [
"//aos:macros",
"//aos/logging",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -178,7 +179,7 @@
"//aos:macros",
"//aos/controls:control_loop",
"//aos/logging",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -189,7 +190,7 @@
],
deps = [
":state_feedback_loop",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -199,7 +200,7 @@
"runge_kutta.h",
],
deps = [
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -211,7 +212,7 @@
deps = [
":runge_kutta",
"//aos/testing:googletest",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -233,24 +234,13 @@
],
)
-queue_library(
- name = "profiled_subsystem_queue",
+flatbuffer_cc_library(
+ name = "profiled_subsystem_fbs",
srcs = [
- "profiled_subsystem.q",
+ "profiled_subsystem.fbs",
],
- deps = [
- ":queues",
- ],
-)
-
-queue_library(
- name = "static_zeroing_single_dof_profiled_subsystem_test_queue",
- srcs = [
- "static_zeroing_single_dof_profiled_subsystem_test.q",
- ],
- deps = [
- ":profiled_subsystem_queue",
- ":queues",
+ includes = [
+ ":control_loops_fbs_includes",
],
)
@@ -263,7 +253,8 @@
"profiled_subsystem.h",
],
deps = [
- ":profiled_subsystem_queue",
+ ":control_loops_fbs",
+ ":profiled_subsystem_fbs",
":simple_capped_state_feedback_loop",
":state_feedback_loop",
"//aos/controls:control_loop",
@@ -278,7 +269,7 @@
"jacobian.h",
],
deps = [
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -290,7 +281,7 @@
deps = [
":jacobian",
"//aos/testing:googletest",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -315,7 +306,7 @@
visibility = ["//visibility:public"],
deps = [
"//aos/time",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -326,7 +317,7 @@
],
visibility = ["//visibility:public"],
deps = [
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
"@slycot_repo//:slicot",
],
)
@@ -408,6 +399,17 @@
],
)
+flatbuffer_cc_library(
+ name = "static_zeroing_single_dof_profiled_subsystem_test_fbs",
+ srcs = [
+ "static_zeroing_single_dof_profiled_subsystem_test.fbs",
+ ],
+ includes = [
+ ":control_loops_fbs_includes",
+ ":profiled_subsystem_fbs_includes",
+ ],
+)
+
cc_test(
name = "static_zeroing_single_dof_profiled_subsystem_test",
srcs = [
@@ -417,8 +419,8 @@
":capped_test_plant",
":position_sensor_sim",
":static_zeroing_single_dof_profiled_subsystem",
+ ":static_zeroing_single_dof_profiled_subsystem_test_fbs",
":static_zeroing_single_dof_profiled_subsystem_test_plants",
- ":static_zeroing_single_dof_profiled_subsystem_test_queue",
"//aos/controls:control_loop_test",
"//aos/testing:googletest",
],
diff --git a/frc971/control_loops/coerce_goal.h b/frc971/control_loops/coerce_goal.h
index 6c1f504..6e927b0 100644
--- a/frc971/control_loops/coerce_goal.h
+++ b/frc971/control_loops/coerce_goal.h
@@ -76,7 +76,7 @@
} else {
Eigen::Matrix<Scalar, 2, 4> region_vertices = region.StaticVertices();
#ifdef __linux__
- AOS_CHECK_GT(region_vertices.outerSize(), 0);
+ CHECK_GT(reinterpret_cast<ssize_t>(region_vertices.outerSize()), 0);
#else
assert(region_vertices.outerSize() > 0);
#endif
diff --git a/frc971/control_loops/control_loops.q b/frc971/control_loops/control_loops.fbs
similarity index 65%
rename from frc971/control_loops/control_loops.q
rename to frc971/control_loops/control_loops.fbs
index 2359bcd..4f1e888 100644
--- a/frc971/control_loops/control_loops.q
+++ b/frc971/control_loops/control_loops.fbs
@@ -1,4 +1,4 @@
-package frc971;
+namespace frc971;
// Represents all of the data for a single indexed encoder. In other words,
// just a relative encoder with an index pulse.
@@ -6,14 +6,14 @@
// All encoder values are relative to where the encoder was at some arbitrary
// point in time. All potentiometer values are relative to some arbitrary 0
// position which varies with each robot.
-struct IndexPosition {
+table IndexPosition {
// Current position read from the encoder.
- double encoder;
+ encoder:double;
// Position from the encoder latched at the last index pulse.
- double latched_encoder;
+ latched_encoder:double;
// How many index pulses we've seen since startup. Starts at 0.
- uint32_t index_pulses;
-};
+ index_pulses:uint;
+}
// Represents all of the data for a single potentiometer and indexed encoder
// pair.
@@ -21,20 +21,20 @@
// All encoder values are relative to where the encoder was at some arbitrary
// point in time. All potentiometer values are relative to some arbitrary 0
// position which varies with each robot.
-struct PotAndIndexPosition {
+table PotAndIndexPosition {
// Current position read from the encoder.
- double encoder;
+ encoder:double;
// Current position read from the potentiometer.
- double pot;
+ pot:double;
// Position from the encoder latched at the last index pulse.
- double latched_encoder;
+ latched_encoder:double;
// Position from the potentiometer latched at the last index pulse.
- double latched_pot;
+ latched_pot:double;
// How many index pulses we've seen since startup. Starts at 0.
- uint32_t index_pulses;
-};
+ index_pulses:uint;
+}
// Represents all of the data for a single potentiometer with an absolute and
// relative encoder pair.
@@ -42,182 +42,183 @@
// The relative encoder values are relative to where the encoder was at some
// arbitrary point in time. All potentiometer values are relative to some
// arbitrary 0 position which varies with each robot.
-struct PotAndAbsolutePosition {
+table PotAndAbsolutePosition {
// Current position read from each encoder.
- double encoder;
- double absolute_encoder;
+ encoder:double;
+ absolute_encoder:double;
// Current position read from the potentiometer.
- double pot;
-};
+ pot:double;
+}
// Represents all of the data for an absolute and relative encoder pair.
// The units on all of the positions are the same.
// The relative encoder values are relative to where the encoder was at some
// arbitrary point in time.
-struct AbsolutePosition {
+table AbsolutePosition {
// Current position read from each encoder.
- double encoder;
- double absolute_encoder;
-};
+ encoder:double;
+ absolute_encoder:double;
+}
// The internal state of a zeroing estimator.
-struct EstimatorState {
+table EstimatorState {
// If true, there has been a fatal error for the estimator.
- bool error;
+ error:bool;
// If the joint has seen an index pulse and is zeroed.
- bool zeroed;
+ zeroed:bool;
// The estimated position of the joint.
- double position;
+ position:double;
// The estimated position not using the index pulse.
- double pot_position;
-};
+ pot_position:double;
+}
// The internal state of a zeroing estimator.
-struct PotAndAbsoluteEncoderEstimatorState {
+table PotAndAbsoluteEncoderEstimatorState {
// If true, there has been a fatal error for the estimator.
- bool error;
+ error:bool;
// If the joint has seen an index pulse and is zeroed.
- bool zeroed;
+ zeroed:bool;
// The estimated position of the joint.
- double position;
+ position:double;
// The estimated position not using the index pulse.
- double pot_position;
+ pot_position:double;
// The estimated absolute position of the encoder. This is filtered, so it
// can be easily used when zeroing.
- double absolute_position;
-};
+ absolute_position:double;
+}
// The internal state of a zeroing estimator.
-struct AbsoluteEncoderEstimatorState {
+table AbsoluteEncoderEstimatorState {
// If true, there has been a fatal error for the estimator.
- bool error;
+ error:bool;
// If the joint has seen an index pulse and is zeroed.
- bool zeroed;
+ zeroed:bool;
// The estimated position of the joint.
- double position;
+ position:double;
// The estimated absolute position of the encoder. This is filtered, so it
// can be easily used when zeroing.
- double absolute_position;
-};
+ absolute_position:double;
+}
// The internal state of a zeroing estimator.
-struct IndexEstimatorState {
+table IndexEstimatorState {
// If true, there has been a fatal error for the estimator.
- bool error;
+ error:bool;
// If the joint has seen an index pulse and is zeroed.
- bool zeroed;
+ zeroed:bool;
// The estimated position of the joint. This is just the position relative to
// where we started if we're not zeroed yet.
- double position;
+ position:double;
// The positions of the extreme index pulses we've seen.
- double min_index_position;
- double max_index_position;
+ min_index_position:double;
+ max_index_position:double;
// The number of index pulses we've seen.
- int32_t index_pulses_seen;
-};
+ index_pulses_seen:int;
+}
-struct HallEffectAndPositionEstimatorState {
+table HallEffectAndPositionEstimatorState {
// If error.
- bool error;
+ error:bool;
// If we've found a positive edge while moving backwards and is zeroed.
- bool zeroed;
+ zeroed:bool;
// Encoder angle relative to where we started.
- double encoder;
+ encoder:double;
// The positions of the extreme posedges we've seen.
// If we've gotten enough samples where the hall effect is high before can be
// certain it is not a false positive.
- bool high_long_enough;
- double offset;
-};
+ high_long_enough:bool;
+ offset:double;
+}
// A left/right pair of PotAndIndexPositions.
-struct PotAndIndexPair {
- PotAndIndexPosition left;
- PotAndIndexPosition right;
-};
+table PotAndIndexPair {
+ left:PotAndIndexPosition;
+ right:PotAndIndexPosition;
+}
// Records edges captured on a single hall effect sensor.
-struct HallEffectStruct {
- bool current;
- int32_t posedge_count;
- int32_t negedge_count;
- double posedge_value;
- double negedge_value;
-};
+table HallEffectStruct {
+ current:bool;
+ posedge_count:int;
+ negedge_count:int;
+ posedge_value:double;
+ negedge_value:double;
+}
// Records the hall effect sensor and encoder values.
-struct HallEffectAndPosition {
+table HallEffectAndPosition {
// The current hall effect state.
- bool current;
+ current:bool;
// The current encoder position.
- double encoder;
+ encoder:double;
// The number of positive and negative edges we've seen on the hall effect
// sensor.
- int32_t posedge_count;
- int32_t negedge_count;
+ posedge_count:int;
+ negedge_count:int;
// The values corresponding to the last hall effect sensor reading.
- double posedge_value;
- double negedge_value;
-};
+ posedge_value:double;
+ negedge_value:double;
+}
// Records the positions for a mechanism with edge-capturing sensors on it.
-struct HallEffectPositions {
- double current;
- double posedge;
- double negedge;
-};
+table HallEventPositions {
+ current:double;
+ posedge:double;
+ negedge:double;
+}
// Records edges captured on a single hall effect sensor.
-struct PosedgeOnlyCountedHallEffectStruct {
- bool current;
- int32_t posedge_count;
- int32_t negedge_count;
- double posedge_value;
-};
+table PosedgeOnlyCountedHallEffectStruct {
+ current:bool;
+ posedge_count:int;
+ negedge_count:int;
+ posedge_value:double;
+}
// Parameters for the motion profiles.
-struct ProfileParameters {
+table ProfileParameters {
// Maximum velocity for the profile.
- float max_velocity;
+ max_velocity:float;
// Maximum acceleration for the profile.
- float max_acceleration;
-};
+ max_acceleration:float;
+}
+
+enum ConstraintType : byte {
+ CONSTRAINT_TYPE_UNDEFINED,
+ LONGITUDINAL_ACCELERATION,
+ LATERAL_ACCELERATION,
+ VOLTAGE,
+ VELOCITY,
+}
// Definition of a constraint on a trajectory
-struct Constraint {
- // Type of constraint
- // 0: Null constraint. Ignore and all following
- // 1: longitual acceleration
- // 2: lateral acceleration
- // 3: voltage
- // 4: velocity
- uint8_t constraint_type;
- float value;
+table Constraint {
+ constraint_type:ConstraintType;
+
+ value:float;
+
// start and end distance are only checked for velocity limits.
- float start_distance;
- float end_distance;
-};
+ start_distance:float;
+ end_distance:float;
+}
// Parameters for computing a trajectory using a chain of splines and
// constraints.
-struct MultiSpline {
- // index of the spline. Zero indicates the spline should not be computed.
- int32_t spline_idx;
+table MultiSpline {
// Number of splines. The spline point arrays will be expected to have
// 6 + 5 * (n - 1) points in them. The endpoints are shared between
// neighboring splines.
- uint8_t spline_count;
- float[36] spline_x;
- float[36] spline_y;
+ spline_count:byte;
+ // Maximum of 36 spline points (7 splines).
+ spline_x:[float];
+ spline_y:[float];
- // Whether to follow the spline driving forwards or backwards.
- bool drive_spline_backwards;
-
- Constraint[6] constraints;
-};
+ // Maximum of 6 constraints;
+ constraints:[Constraint];
+}
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"
diff --git a/frc971/control_loops/hall_effect_tracker.h b/frc971/control_loops/hall_effect_tracker.h
index d1b6efe..084c9a7 100644
--- a/frc971/control_loops/hall_effect_tracker.h
+++ b/frc971/control_loops/hall_effect_tracker.h
@@ -3,7 +3,7 @@
#include <stdint.h>
-#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
namespace frc971 {
@@ -27,22 +27,22 @@
double posedge_value() const { return posedge_value_; }
double negedge_value() const { return negedge_value_; }
- void Update(const HallEffectStruct &position) {
+ void Update(const HallEffectStruct *position) {
last_value_ = value_;
- value_ = position.current;
- posedge_value_ = position.posedge_value;
- negedge_value_ = position.negedge_value;
- posedges_.update(position.posedge_count);
- negedges_.update(position.negedge_count);
+ value_ = position->current();
+ posedge_value_ = position->posedge_value();
+ negedge_value_ = position->negedge_value();
+ posedges_.update(position->posedge_count());
+ negedges_.update(position->negedge_count());
}
- void Reset(const HallEffectStruct &position) {
- posedges_.Reset(position.posedge_count);
- negedges_.Reset(position.negedge_count);
- value_ = position.current;
- last_value_ = position.current;
- posedge_value_ = position.posedge_value;
- negedge_value_ = position.negedge_value;
+ void Reset(const HallEffectStruct *position) {
+ posedges_.Reset(position->posedge_count());
+ negedges_.Reset(position->negedge_count());
+ value_ = position->current();
+ last_value_ = position->current();
+ posedge_value_ = position->posedge_value();
+ negedge_value_ = position->negedge_value();
}
private:
diff --git a/frc971/control_loops/position_sensor_sim.cc b/frc971/control_loops/position_sensor_sim.cc
index c875a0b..612801e 100644
--- a/frc971/control_loops/position_sensor_sim.cc
+++ b/frc971/control_loops/position_sensor_sim.cc
@@ -127,72 +127,95 @@
current_position_ = new_position;
}
-void PositionSensorSimulator::GetSensorValues(IndexPosition *values) {
- values->encoder = current_position_ - start_position_;
+template <>
+flatbuffers::Offset<IndexPosition>
+PositionSensorSimulator::GetSensorValues<IndexPositionBuilder>(
+ IndexPositionBuilder *builder) {
+ builder->add_encoder(current_position_ - start_position_);
- values->index_pulses = lower_index_edge_.index_count();
- if (values->index_pulses == 0) {
- values->latched_encoder = 0.0;
+ const int index_count = lower_index_edge_.index_count();
+ builder->add_index_pulses(index_count);
+ if (index_count == 0) {
+ builder->add_latched_encoder(0.0);
} else {
// Populate the latched encoder samples.
- values->latched_encoder =
- lower_index_edge_.IndexPulsePosition() - start_position_;
+ builder->add_latched_encoder(lower_index_edge_.IndexPulsePosition() -
+ start_position_);
}
+ return builder->Finish();
}
-void PositionSensorSimulator::GetSensorValues(PotAndIndexPosition *values) {
- values->pot = lower_index_edge_.mutable_pot_noise()->AddNoiseToSample(
- current_position_);
- values->encoder = current_position_ - start_position_;
+template <>
+flatbuffers::Offset<PotAndIndexPosition>
+PositionSensorSimulator::GetSensorValues<PotAndIndexPositionBuilder>(
+ PotAndIndexPositionBuilder *builder) {
+ builder->add_pot(lower_index_edge_.mutable_pot_noise()->AddNoiseToSample(
+ current_position_));
+ builder->add_encoder(current_position_ - start_position_);
if (lower_index_edge_.index_count() == 0) {
- values->latched_pot = 0.0;
- values->latched_encoder = 0.0;
+ builder->add_latched_pot(0.0);
+ builder->add_latched_encoder(0.0);
} else {
// Populate the latched pot/encoder samples.
- values->latched_pot = lower_index_edge_.latched_pot();
- values->latched_encoder =
- lower_index_edge_.IndexPulsePosition() - start_position_;
+ builder->add_latched_pot(lower_index_edge_.latched_pot());
+ builder->add_latched_encoder(lower_index_edge_.IndexPulsePosition() -
+ start_position_);
}
- values->index_pulses = lower_index_edge_.index_count();
+ builder->add_index_pulses(lower_index_edge_.index_count());
+ return builder->Finish();
}
-void PositionSensorSimulator::GetSensorValues(PotAndAbsolutePosition *values) {
- values->pot = lower_index_edge_.mutable_pot_noise()->AddNoiseToSample(
- current_position_);
- values->encoder = current_position_ - start_position_;
+template <>
+flatbuffers::Offset<PotAndAbsolutePosition>
+PositionSensorSimulator::GetSensorValues<PotAndAbsolutePositionBuilder>(
+ PotAndAbsolutePositionBuilder *builder) {
+ builder->add_pot(lower_index_edge_.mutable_pot_noise()->AddNoiseToSample(
+ current_position_));
+ builder->add_encoder(current_position_ - start_position_);
// TODO(phil): Create some lag here since this is a PWM signal it won't be
// instantaneous like the other signals. Better yet, its lag varies
// randomly with the distribution varying depending on the reading.
- values->absolute_encoder = ::std::remainder(
+ double absolute_encoder = ::std::remainder(
current_position_ + known_absolute_encoder_, distance_per_revolution_);
- if (values->absolute_encoder < 0) {
- values->absolute_encoder += distance_per_revolution_;
+ if (absolute_encoder < 0) {
+ absolute_encoder += distance_per_revolution_;
}
+ builder->add_absolute_encoder(absolute_encoder);
+ return builder->Finish();
}
-void PositionSensorSimulator::GetSensorValues(AbsolutePosition *values) {
- values->encoder = current_position_ - start_position_;
+template <>
+flatbuffers::Offset<AbsolutePosition>
+PositionSensorSimulator::GetSensorValues<AbsolutePositionBuilder>(
+ AbsolutePositionBuilder *builder) {
+ builder->add_encoder(current_position_ - start_position_);
// TODO(phil): Create some lag here since this is a PWM signal it won't be
// instantaneous like the other signals. Better yet, its lag varies
// randomly with the distribution varying depending on the reading.
- values->absolute_encoder = ::std::remainder(
+ double absolute_encoder = ::std::remainder(
current_position_ + known_absolute_encoder_, distance_per_revolution_);
- if (values->absolute_encoder < 0) {
- values->absolute_encoder += distance_per_revolution_;
+ if (absolute_encoder < 0) {
+ absolute_encoder += distance_per_revolution_;
}
+ builder->add_absolute_encoder(absolute_encoder);
+ return builder->Finish();
}
-void PositionSensorSimulator::GetSensorValues(HallEffectAndPosition *values) {
- values->current = lower_index_edge_.current_index_segment() !=
- upper_index_edge_.current_index_segment();
- values->encoder = current_position_ - start_position_;
+template <>
+flatbuffers::Offset<HallEffectAndPosition>
+PositionSensorSimulator::GetSensorValues<HallEffectAndPositionBuilder>(
+ HallEffectAndPositionBuilder *builder) {
+ builder->add_current(lower_index_edge_.current_index_segment() !=
+ upper_index_edge_.current_index_segment());
+ builder->add_encoder(current_position_ - start_position_);
- values->posedge_count = posedge_count_;
- values->negedge_count = negedge_count_;
- values->posedge_value = posedge_value_ - start_position_;
- values->negedge_value = negedge_value_ - start_position_;
+ builder->add_posedge_count(posedge_count_);
+ builder->add_negedge_count(negedge_count_);
+ builder->add_posedge_value(posedge_value_ - start_position_);
+ builder->add_negedge_value(negedge_value_ - start_position_);
+ return builder->Finish();
}
} // namespace control_loops
diff --git a/frc971/control_loops/position_sensor_sim.h b/frc971/control_loops/position_sensor_sim.h
index 2880429..6be6bbc 100644
--- a/frc971/control_loops/position_sensor_sim.h
+++ b/frc971/control_loops/position_sensor_sim.h
@@ -1,9 +1,11 @@
#ifndef FRC971_CONTROL_LOOPS_POSITION_SENSOR_SIM_H_
#define FRC971_CONTROL_LOOPS_POSITION_SENSOR_SIM_H_
+#include "flatbuffers/flatbuffers.h"
+
#include "aos/testing/random_seed.h"
-#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/gaussian_noise.h"
namespace frc971 {
@@ -58,11 +60,17 @@
// values: The target structure will be populated with simulated sensor
// readings. The readings will be in SI units. For example the units
// can be given in radians, meters, etc.
- void GetSensorValues(IndexPosition *values);
- void GetSensorValues(PotAndIndexPosition *values);
- void GetSensorValues(AbsolutePosition *values);
- void GetSensorValues(PotAndAbsolutePosition *values);
- void GetSensorValues(HallEffectAndPosition *values);
+ template <typename PositionBuilder>
+ flatbuffers::Offset<typename PositionBuilder::Table> GetSensorValues(
+ PositionBuilder *builder);
+ template <typename Position>
+ const Position *FillSensorValues(flatbuffers::FlatBufferBuilder *fbb) {
+ fbb->Clear();
+ typename Position::Builder values(*fbb);
+
+ fbb->Finish(GetSensorValues(&values));
+ return flatbuffers::GetRoot<Position>(fbb->GetBufferPointer());
+ }
private:
// It turns out that we can re-use a lot of the same logic to count the index
@@ -84,7 +92,7 @@
index_count_ = 0;
}
- double index_count() const { return index_count_; }
+ int index_count() const { return index_count_; }
double latched_pot() const { return latched_pot_; }
int current_index_segment() const { return current_index_segment_; }
diff --git a/frc971/control_loops/position_sensor_sim_test.cc b/frc971/control_loops/position_sensor_sim_test.cc
index a4436ce..93a648a 100644
--- a/frc971/control_loops/position_sensor_sim_test.cc
+++ b/frc971/control_loops/position_sensor_sim_test.cc
@@ -5,10 +5,12 @@
#include <random>
#include "gtest/gtest.h"
-#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "aos/die.h"
+#include "flatbuffers/flatbuffers.h"
+
namespace frc971 {
namespace control_loops {
@@ -23,46 +25,51 @@
// this test is to verify that no false index pulses are generated while the
// mechanism stays between two index pulses.
const double index_diff = 0.5;
- IndexPosition index_position;
- PotAndIndexPosition pot_and_index_position;
+ flatbuffers::FlatBufferBuilder fbb;
+ flatbuffers::FlatBufferBuilder pot_fbb;
PositionSensorSimulator sim(index_diff);
sim.Initialize(3.6 * index_diff, 0);
// Make sure that we don't accidentally hit an index pulse.
for (int i = 0; i < 30; i++) {
sim.MoveTo(3.6 * index_diff);
- sim.GetSensorValues(&index_position);
- sim.GetSensorValues(&pot_and_index_position);
- ASSERT_DOUBLE_EQ(3.6 * index_diff, pot_and_index_position.pot);
- ASSERT_EQ(0u, pot_and_index_position.index_pulses);
- ASSERT_EQ(0u, index_position.index_pulses);
+ const IndexPosition *index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ const PotAndIndexPosition *pot_and_index_position =
+ sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ ASSERT_DOUBLE_EQ(3.6 * index_diff, pot_and_index_position->pot());
+ ASSERT_EQ(0u, pot_and_index_position->index_pulses());
+
+ ASSERT_EQ(0u, index_position->index_pulses());
}
for (int i = 0; i < 30; i++) {
sim.MoveTo(3.0 * index_diff);
- sim.GetSensorValues(&index_position);
- sim.GetSensorValues(&pot_and_index_position);
- ASSERT_DOUBLE_EQ(3.0 * index_diff, pot_and_index_position.pot);
- ASSERT_EQ(0u, pot_and_index_position.index_pulses);
- ASSERT_EQ(0u, index_position.index_pulses);
+ const IndexPosition *index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ const PotAndIndexPosition *pot_and_index_position =
+ sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ ASSERT_DOUBLE_EQ(3.0 * index_diff, pot_and_index_position->pot());
+ ASSERT_EQ(0u, pot_and_index_position->index_pulses());
+ ASSERT_EQ(0u, index_position->index_pulses());
}
for (int i = 0; i < 30; i++) {
sim.MoveTo(3.99 * index_diff);
- sim.GetSensorValues(&index_position);
- sim.GetSensorValues(&pot_and_index_position);
- ASSERT_DOUBLE_EQ(3.99 * index_diff, pot_and_index_position.pot);
- ASSERT_EQ(0u, pot_and_index_position.index_pulses);
- ASSERT_EQ(0u, index_position.index_pulses);
+ const IndexPosition *index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ const PotAndIndexPosition *pot_and_index_position =
+ sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ ASSERT_DOUBLE_EQ(3.99 * index_diff, pot_and_index_position->pot());
+ ASSERT_EQ(0u, pot_and_index_position->index_pulses());
+ ASSERT_EQ(0u, index_position->index_pulses());
}
for (int i = 0; i < 30; i++) {
sim.MoveTo(3.0 * index_diff);
- sim.GetSensorValues(&index_position);
- sim.GetSensorValues(&pot_and_index_position);
- ASSERT_DOUBLE_EQ(3.0 * index_diff, pot_and_index_position.pot);
- ASSERT_EQ(0u, pot_and_index_position.index_pulses);
- ASSERT_EQ(0u, index_position.index_pulses);
+ const IndexPosition *index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ const PotAndIndexPosition *pot_and_index_position =
+ sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ ASSERT_DOUBLE_EQ(3.0 * index_diff, pot_and_index_position->pot());
+ ASSERT_EQ(0u, pot_and_index_position->index_pulses());
+ ASSERT_EQ(0u, index_position->index_pulses());
}
}
@@ -72,58 +79,59 @@
// again simulate zero noise on the potentiometer to accurately verify the
// mechanism's position during the index pulses.
const double index_diff = 0.8;
- IndexPosition index_position;
- PotAndIndexPosition pot_and_index_position;
+ flatbuffers::FlatBufferBuilder fbb;
+ flatbuffers::FlatBufferBuilder pot_fbb;
PositionSensorSimulator sim(index_diff);
sim.Initialize(4.6 * index_diff, 0);
// Make sure that we get an index pulse on every transition.
- sim.GetSensorValues(&index_position);
- sim.GetSensorValues(&pot_and_index_position);
- ASSERT_EQ(0u, index_position.index_pulses);
- ASSERT_EQ(0u, pot_and_index_position.index_pulses);
+ const IndexPosition *index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ const PotAndIndexPosition *pot_and_index_position =
+ sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ ASSERT_EQ(0u, index_position->index_pulses());
+ ASSERT_EQ(0u, pot_and_index_position->index_pulses());
sim.MoveTo(3.6 * index_diff);
- sim.GetSensorValues(&index_position);
- sim.GetSensorValues(&pot_and_index_position);
- ASSERT_DOUBLE_EQ(4.0 * index_diff, pot_and_index_position.latched_pot);
- ASSERT_EQ(1u, index_position.index_pulses);
- ASSERT_EQ(1u, pot_and_index_position.index_pulses);
+ index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ pot_and_index_position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ ASSERT_DOUBLE_EQ(4.0 * index_diff, pot_and_index_position->latched_pot());
+ ASSERT_EQ(1u, index_position->index_pulses());
+ ASSERT_EQ(1u, pot_and_index_position->index_pulses());
sim.MoveTo(4.5 * index_diff);
- sim.GetSensorValues(&index_position);
- sim.GetSensorValues(&pot_and_index_position);
- ASSERT_DOUBLE_EQ(4.0 * index_diff, pot_and_index_position.latched_pot);
- ASSERT_EQ(2u, index_position.index_pulses);
- ASSERT_EQ(2u, pot_and_index_position.index_pulses);
+ index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ pot_and_index_position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ ASSERT_DOUBLE_EQ(4.0 * index_diff, pot_and_index_position->latched_pot());
+ ASSERT_EQ(2u, index_position->index_pulses());
+ ASSERT_EQ(2u, pot_and_index_position->index_pulses());
sim.MoveTo(5.9 * index_diff);
- sim.GetSensorValues(&index_position);
- sim.GetSensorValues(&pot_and_index_position);
- ASSERT_DOUBLE_EQ(5.0 * index_diff, pot_and_index_position.latched_pot);
- ASSERT_EQ(3u, index_position.index_pulses);
- ASSERT_EQ(3u, pot_and_index_position.index_pulses);
+ index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ pot_and_index_position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ ASSERT_DOUBLE_EQ(5.0 * index_diff, pot_and_index_position->latched_pot());
+ ASSERT_EQ(3u, index_position->index_pulses());
+ ASSERT_EQ(3u, pot_and_index_position->index_pulses());
sim.MoveTo(6.1 * index_diff);
- sim.GetSensorValues(&index_position);
- sim.GetSensorValues(&pot_and_index_position);
- ASSERT_DOUBLE_EQ(6.0 * index_diff, pot_and_index_position.latched_pot);
- ASSERT_EQ(4u, index_position.index_pulses);
- ASSERT_EQ(4u, pot_and_index_position.index_pulses);
+ index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ pot_and_index_position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ ASSERT_DOUBLE_EQ(6.0 * index_diff, pot_and_index_position->latched_pot());
+ ASSERT_EQ(4u, index_position->index_pulses());
+ ASSERT_EQ(4u, pot_and_index_position->index_pulses());
sim.MoveTo(8.7 * index_diff);
- sim.GetSensorValues(&index_position);
- sim.GetSensorValues(&pot_and_index_position);
- ASSERT_DOUBLE_EQ(8.0 * index_diff, pot_and_index_position.latched_pot);
- ASSERT_EQ(5u, index_position.index_pulses);
- ASSERT_EQ(5u, pot_and_index_position.index_pulses);
+ index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ pot_and_index_position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ ASSERT_DOUBLE_EQ(8.0 * index_diff, pot_and_index_position->latched_pot());
+ ASSERT_EQ(5u, index_position->index_pulses());
+ ASSERT_EQ(5u, pot_and_index_position->index_pulses());
sim.MoveTo(7.3 * index_diff);
- sim.GetSensorValues(&index_position);
- sim.GetSensorValues(&pot_and_index_position);
- ASSERT_DOUBLE_EQ(8.0 * index_diff, pot_and_index_position.latched_pot);
- ASSERT_EQ(6u, index_position.index_pulses);
- ASSERT_EQ(6u, pot_and_index_position.index_pulses);
+ index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ pot_and_index_position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ ASSERT_DOUBLE_EQ(8.0 * index_diff, pot_and_index_position->latched_pot());
+ ASSERT_EQ(6u, index_position->index_pulses());
+ ASSERT_EQ(6u, pot_and_index_position->index_pulses());
}
// Tests that the simulator handles non-zero specified index pulse locations
@@ -132,65 +140,66 @@
const double index_diff = 0.5;
PositionSensorSimulator sim(index_diff);
sim.Initialize(index_diff * 0.25, 0.0, index_diff * 0.5);
- IndexPosition index_position;
- PotAndIndexPosition pot_and_index_position;
+ flatbuffers::FlatBufferBuilder fbb;
+ flatbuffers::FlatBufferBuilder pot_fbb;
sim.MoveTo(0.75 * index_diff);
- sim.GetSensorValues(&index_position);
- sim.GetSensorValues(&pot_and_index_position);
- EXPECT_EQ(1u, index_position.index_pulses);
- EXPECT_EQ(1u, pot_and_index_position.index_pulses);
- EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position.latched_pot);
- EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position.latched_encoder);
- EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position.latched_encoder);
+ const IndexPosition *index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ const PotAndIndexPosition *pot_and_index_position =
+ sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ EXPECT_EQ(1u, index_position->index_pulses());
+ EXPECT_EQ(1u, pot_and_index_position->index_pulses());
+ EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position->latched_pot());
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position->latched_encoder());
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position->latched_encoder());
sim.MoveTo(index_diff);
- sim.GetSensorValues(&index_position);
- sim.GetSensorValues(&pot_and_index_position);
- EXPECT_EQ(1u, index_position.index_pulses);
- EXPECT_EQ(1u, pot_and_index_position.index_pulses);
- EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position.latched_pot);
- EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position.latched_encoder);
- EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position.latched_encoder);
+ index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ pot_and_index_position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ EXPECT_EQ(1u, index_position->index_pulses());
+ EXPECT_EQ(1u, pot_and_index_position->index_pulses());
+ EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position->latched_pot());
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position->latched_encoder());
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position->latched_encoder());
sim.MoveTo(1.75 * index_diff);
- sim.GetSensorValues(&index_position);
- sim.GetSensorValues(&pot_and_index_position);
- EXPECT_EQ(2u, index_position.index_pulses);
- EXPECT_EQ(2u, pot_and_index_position.index_pulses);
- EXPECT_DOUBLE_EQ(index_diff * 1.5, pot_and_index_position.latched_pot);
- EXPECT_DOUBLE_EQ(index_diff * 1.25, index_position.latched_encoder);
- EXPECT_DOUBLE_EQ(index_diff * 1.25, pot_and_index_position.latched_encoder);
+ index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ pot_and_index_position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ EXPECT_EQ(2u, index_position->index_pulses());
+ EXPECT_EQ(2u, pot_and_index_position->index_pulses());
+ EXPECT_DOUBLE_EQ(index_diff * 1.5, pot_and_index_position->latched_pot());
+ EXPECT_DOUBLE_EQ(index_diff * 1.25, index_position->latched_encoder());
+ EXPECT_DOUBLE_EQ(index_diff * 1.25, pot_and_index_position->latched_encoder());
// Try it with our known index pulse not being our first one.
sim.Initialize(index_diff * 0.25, 0.0, index_diff * 1.5);
sim.MoveTo(0.75 * index_diff);
- sim.GetSensorValues(&index_position);
- sim.GetSensorValues(&pot_and_index_position);
- EXPECT_EQ(1u, index_position.index_pulses);
- EXPECT_EQ(1u, pot_and_index_position.index_pulses);
- EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position.latched_pot);
- EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position.latched_encoder);
- EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position.latched_encoder);
+ index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ pot_and_index_position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ EXPECT_EQ(1u, index_position->index_pulses());
+ EXPECT_EQ(1u, pot_and_index_position->index_pulses());
+ EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position->latched_pot());
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position->latched_encoder());
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position->latched_encoder());
sim.MoveTo(index_diff);
- sim.GetSensorValues(&index_position);
- sim.GetSensorValues(&pot_and_index_position);
- EXPECT_EQ(1u, index_position.index_pulses);
- EXPECT_EQ(1u, pot_and_index_position.index_pulses);
- EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position.latched_pot);
- EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position.latched_encoder);
- EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position.latched_encoder);
+ index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ pot_and_index_position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ EXPECT_EQ(1u, index_position->index_pulses());
+ EXPECT_EQ(1u, pot_and_index_position->index_pulses());
+ EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position->latched_pot());
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position->latched_encoder());
+ EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position->latched_encoder());
sim.MoveTo(1.75 * index_diff);
- sim.GetSensorValues(&index_position);
- sim.GetSensorValues(&pot_and_index_position);
- EXPECT_EQ(2u, index_position.index_pulses);
- EXPECT_EQ(2u, pot_and_index_position.index_pulses);
- EXPECT_DOUBLE_EQ(index_diff * 1.5, pot_and_index_position.latched_pot);
- EXPECT_DOUBLE_EQ(index_diff * 1.25, index_position.latched_encoder);
- EXPECT_DOUBLE_EQ(index_diff * 1.25, pot_and_index_position.latched_encoder);
+ index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ pot_and_index_position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ EXPECT_EQ(2u, index_position->index_pulses());
+ EXPECT_EQ(2u, pot_and_index_position->index_pulses());
+ EXPECT_DOUBLE_EQ(index_diff * 1.5, pot_and_index_position->latched_pot());
+ EXPECT_DOUBLE_EQ(index_diff * 1.25, index_position->latched_encoder());
+ EXPECT_DOUBLE_EQ(index_diff * 1.25, pot_and_index_position->latched_encoder());
}
// Tests that the latched values update correctly.
@@ -198,44 +207,45 @@
const double index_diff = 0.5;
PositionSensorSimulator sim(index_diff);
sim.Initialize(0, 0.25);
- PotAndIndexPosition position;
+ flatbuffers::FlatBufferBuilder pot_fbb;
sim.MoveTo(0.75 * index_diff);
- sim.GetSensorValues(&position);
- EXPECT_EQ(0u, position.index_pulses);
+ const PotAndIndexPosition *position =
+ sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ EXPECT_EQ(0u, position->index_pulses());
sim.MoveTo(1.75 * index_diff);
- sim.GetSensorValues(&position);
- EXPECT_EQ(1u, position.index_pulses);
- EXPECT_NEAR(index_diff, position.latched_pot, 0.75);
- EXPECT_DOUBLE_EQ(index_diff, position.latched_encoder);
- const double first_latched_pot = position.latched_pot;
+ position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ EXPECT_EQ(1u, position->index_pulses());
+ EXPECT_NEAR(index_diff, position->latched_pot(), 0.75);
+ EXPECT_DOUBLE_EQ(index_diff, position->latched_encoder());
+ const double first_latched_pot = position->latched_pot();
sim.MoveTo(1.95 * index_diff);
- sim.GetSensorValues(&position);
- EXPECT_EQ(1u, position.index_pulses);
- EXPECT_NEAR(index_diff, position.latched_pot, 0.75);
- EXPECT_DOUBLE_EQ(first_latched_pot, position.latched_pot);
- EXPECT_DOUBLE_EQ(index_diff, position.latched_encoder);
+ position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ EXPECT_EQ(1u, position->index_pulses());
+ EXPECT_NEAR(index_diff, position->latched_pot(), 0.75);
+ EXPECT_DOUBLE_EQ(first_latched_pot, position->latched_pot());
+ EXPECT_DOUBLE_EQ(index_diff, position->latched_encoder());
sim.MoveTo(2.05 * index_diff);
- sim.GetSensorValues(&position);
- EXPECT_EQ(2u, position.index_pulses);
- EXPECT_NEAR(index_diff * 2, position.latched_pot, 0.75);
- EXPECT_DOUBLE_EQ(index_diff * 2, position.latched_encoder);
+ position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ EXPECT_EQ(2u, position->index_pulses());
+ EXPECT_NEAR(index_diff * 2, position->latched_pot(), 0.75);
+ EXPECT_DOUBLE_EQ(index_diff * 2, position->latched_encoder());
sim.MoveTo(1.95 * index_diff);
- sim.GetSensorValues(&position);
- EXPECT_EQ(3u, position.index_pulses);
- EXPECT_NEAR(index_diff * 2, position.latched_pot, 0.75);
- EXPECT_DOUBLE_EQ(index_diff * 2, position.latched_encoder);
+ position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ EXPECT_EQ(3u, position->index_pulses());
+ EXPECT_NEAR(index_diff * 2, position->latched_pot(), 0.75);
+ EXPECT_DOUBLE_EQ(index_diff * 2, position->latched_encoder());
sim.MoveTo(0.95 * index_diff);
- sim.GetSensorValues(&position);
- EXPECT_EQ(4u, position.index_pulses);
- EXPECT_NEAR(index_diff, position.latched_pot, 0.75);
- EXPECT_GT(::std::abs(first_latched_pot - position.latched_pot), 0.005);
- EXPECT_DOUBLE_EQ(index_diff, position.latched_encoder);
+ position = sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
+ EXPECT_EQ(4u, position->index_pulses());
+ EXPECT_NEAR(index_diff, position->latched_pot(), 0.75);
+ EXPECT_GT(::std::abs(first_latched_pot - position->latched_pot()), 0.005);
+ EXPECT_DOUBLE_EQ(index_diff, position->latched_encoder());
}
// This test makes sure that our simulation for an absolute encoder + relative
@@ -263,42 +273,43 @@
const double index_diff = 0.1;
PositionSensorSimulator sim(index_diff);
sim.Initialize(0.20, 0.05, 0.2, 0.07);
- PotAndAbsolutePosition position;
+ flatbuffers::FlatBufferBuilder pot_fbb;
sim.MoveTo(0.20);
- sim.GetSensorValues(&position);
- EXPECT_DOUBLE_EQ(0.00, position.encoder);
- EXPECT_NEAR(0.07, position.absolute_encoder, 0.00000001);
+ const PotAndAbsolutePosition *position =
+ sim.FillSensorValues<PotAndAbsolutePosition>(&pot_fbb);
+ EXPECT_DOUBLE_EQ(0.00, position->encoder());
+ EXPECT_NEAR(0.07, position->absolute_encoder(), 0.00000001);
sim.MoveTo(0.30);
- sim.GetSensorValues(&position);
- EXPECT_DOUBLE_EQ(0.10, position.encoder);
- EXPECT_NEAR(0.07, position.absolute_encoder, 0.00000001);
+ position = sim.FillSensorValues<PotAndAbsolutePosition>(&pot_fbb);
+ EXPECT_DOUBLE_EQ(0.10, position->encoder());
+ EXPECT_NEAR(0.07, position->absolute_encoder(), 0.00000001);
sim.MoveTo(0.40);
- sim.GetSensorValues(&position);
- EXPECT_DOUBLE_EQ(0.20, position.encoder);
- EXPECT_NEAR(0.07, position.absolute_encoder, 0.00000001);
+ position = sim.FillSensorValues<PotAndAbsolutePosition>(&pot_fbb);
+ EXPECT_DOUBLE_EQ(0.20, position->encoder());
+ EXPECT_NEAR(0.07, position->absolute_encoder(), 0.00000001);
sim.MoveTo(0.34);
- sim.GetSensorValues(&position);
- EXPECT_DOUBLE_EQ(0.14, position.encoder);
- EXPECT_NEAR(0.01, position.absolute_encoder, 0.00000001);
+ position = sim.FillSensorValues<PotAndAbsolutePosition>(&pot_fbb);
+ EXPECT_DOUBLE_EQ(0.14, position->encoder());
+ EXPECT_NEAR(0.01, position->absolute_encoder(), 0.00000001);
sim.MoveTo(0.24);
- sim.GetSensorValues(&position);
- EXPECT_DOUBLE_EQ(0.04, position.encoder);
- EXPECT_NEAR(0.01, position.absolute_encoder, 0.00000001);
+ position = sim.FillSensorValues<PotAndAbsolutePosition>(&pot_fbb);
+ EXPECT_DOUBLE_EQ(0.04, position->encoder());
+ EXPECT_NEAR(0.01, position->absolute_encoder(), 0.00000001);
sim.MoveTo(0.23);
- sim.GetSensorValues(&position);
- EXPECT_DOUBLE_EQ(0.03, position.encoder);
- EXPECT_NEAR(0.00, position.absolute_encoder, 0.00000001);
+ position = sim.FillSensorValues<PotAndAbsolutePosition>(&pot_fbb);
+ EXPECT_DOUBLE_EQ(0.03, position->encoder());
+ EXPECT_NEAR(0.00, position->absolute_encoder(), 0.00000001);
sim.MoveTo(0.13);
- sim.GetSensorValues(&position);
- EXPECT_DOUBLE_EQ(-0.07, position.encoder);
- EXPECT_NEAR(0.00, position.absolute_encoder, 0.00000001);
+ position = sim.FillSensorValues<PotAndAbsolutePosition>(&pot_fbb);
+ EXPECT_DOUBLE_EQ(-0.07, position->encoder());
+ EXPECT_NEAR(0.00, position->absolute_encoder(), 0.00000001);
// TODO(philipp): Test negative values.
}
@@ -309,78 +320,80 @@
const double index_diff = 1.0;
PositionSensorSimulator sim(index_diff);
sim.InitializeHallEffectAndPosition(-0.25, 0.0, 0.5);
- HallEffectAndPosition position;
+ //HallEffectAndPosition position;
+ flatbuffers::FlatBufferBuilder fbb;
// Go over only the lower edge rising.
sim.MoveTo(0.25);
- sim.GetSensorValues(&position);
- EXPECT_TRUE(position.current);
- EXPECT_DOUBLE_EQ(0.50, position.encoder);
- EXPECT_EQ(1, position.posedge_count);
- EXPECT_EQ(0.25, position.posedge_value);
- EXPECT_EQ(0, position.negedge_count);
- EXPECT_EQ(0, position.negedge_value);
+ const HallEffectAndPosition *position =
+ sim.FillSensorValues<HallEffectAndPosition>(&fbb);
+ EXPECT_TRUE(position->current());
+ EXPECT_DOUBLE_EQ(0.50, position->encoder());
+ EXPECT_EQ(1, position->posedge_count());
+ EXPECT_EQ(0.25, position->posedge_value());
+ EXPECT_EQ(0, position->negedge_count());
+ EXPECT_EQ(0, position->negedge_value());
// Now, go over the upper edge, falling.
sim.MoveTo(0.75);
- sim.GetSensorValues(&position);
- EXPECT_FALSE(position.current);
- EXPECT_DOUBLE_EQ(1.0, position.encoder);
- EXPECT_EQ(1, position.posedge_count);
- EXPECT_DOUBLE_EQ(0.25, position.posedge_value);
- EXPECT_EQ(1, position.negedge_count);
- EXPECT_DOUBLE_EQ(0.75, position.negedge_value);
+ position = sim.FillSensorValues<HallEffectAndPosition>(&fbb);
+ EXPECT_FALSE(position->current());
+ EXPECT_DOUBLE_EQ(1.0, position->encoder());
+ EXPECT_EQ(1, position->posedge_count());
+ EXPECT_DOUBLE_EQ(0.25, position->posedge_value());
+ EXPECT_EQ(1, position->negedge_count());
+ EXPECT_DOUBLE_EQ(0.75, position->negedge_value());
// Now, jump a whole cycle.
sim.MoveTo(1.75);
- sim.GetSensorValues(&position);
- EXPECT_FALSE(position.current);
- EXPECT_DOUBLE_EQ(2.0, position.encoder);
- EXPECT_EQ(2, position.posedge_count);
- EXPECT_DOUBLE_EQ(1.25, position.posedge_value);
- EXPECT_EQ(2, position.negedge_count);
- EXPECT_DOUBLE_EQ(1.75, position.negedge_value);
+ position = sim.FillSensorValues<HallEffectAndPosition>(&fbb);
+ EXPECT_FALSE(position->current());
+ EXPECT_DOUBLE_EQ(2.0, position->encoder());
+ EXPECT_EQ(2, position->posedge_count());
+ EXPECT_DOUBLE_EQ(1.25, position->posedge_value());
+ EXPECT_EQ(2, position->negedge_count());
+ EXPECT_DOUBLE_EQ(1.75, position->negedge_value());
// Now, jump a whole cycle backwards.
sim.MoveTo(0.75);
- sim.GetSensorValues(&position);
- EXPECT_FALSE(position.current);
- EXPECT_DOUBLE_EQ(1.0, position.encoder);
- EXPECT_EQ(3, position.posedge_count);
- EXPECT_DOUBLE_EQ(1.75, position.posedge_value);
- EXPECT_EQ(3, position.negedge_count);
- EXPECT_DOUBLE_EQ(1.25, position.negedge_value);
+ position = sim.FillSensorValues<HallEffectAndPosition>(&fbb);
+ EXPECT_FALSE(position->current());
+ EXPECT_DOUBLE_EQ(1.0, position->encoder());
+ EXPECT_EQ(3, position->posedge_count());
+ EXPECT_DOUBLE_EQ(1.75, position->posedge_value());
+ EXPECT_EQ(3, position->negedge_count());
+ EXPECT_DOUBLE_EQ(1.25, position->negedge_value());
// Now, go over the upper edge, rising.
sim.MoveTo(0.25);
- sim.GetSensorValues(&position);
- EXPECT_TRUE(position.current);
- EXPECT_DOUBLE_EQ(0.5, position.encoder);
- EXPECT_EQ(4, position.posedge_count);
- EXPECT_DOUBLE_EQ(0.75, position.posedge_value);
- EXPECT_EQ(3, position.negedge_count);
- EXPECT_DOUBLE_EQ(1.25, position.negedge_value);
+ position = sim.FillSensorValues<HallEffectAndPosition>(&fbb);
+ EXPECT_TRUE(position->current());
+ EXPECT_DOUBLE_EQ(0.5, position->encoder());
+ EXPECT_EQ(4, position->posedge_count());
+ EXPECT_DOUBLE_EQ(0.75, position->posedge_value());
+ EXPECT_EQ(3, position->negedge_count());
+ EXPECT_DOUBLE_EQ(1.25, position->negedge_value());
// Now, go over the lower edge, falling.
sim.MoveTo(-0.25);
- sim.GetSensorValues(&position);
- EXPECT_FALSE(position.current);
- EXPECT_DOUBLE_EQ(0.0, position.encoder);
- EXPECT_EQ(4, position.posedge_count);
- EXPECT_DOUBLE_EQ(0.75, position.posedge_value);
- EXPECT_EQ(4, position.negedge_count);
- EXPECT_DOUBLE_EQ(0.25, position.negedge_value);
+ position = sim.FillSensorValues<HallEffectAndPosition>(&fbb);
+ EXPECT_FALSE(position->current());
+ EXPECT_DOUBLE_EQ(0.0, position->encoder());
+ EXPECT_EQ(4, position->posedge_count());
+ EXPECT_DOUBLE_EQ(0.75, position->posedge_value());
+ EXPECT_EQ(4, position->negedge_count());
+ EXPECT_DOUBLE_EQ(0.25, position->negedge_value());
for (int i = 0; i < 10; ++i) {
// Now, go over the lower edge, falling.
sim.MoveTo(-0.25 - i * 1.0e-6);
- sim.GetSensorValues(&position);
- EXPECT_FALSE(position.current);
- EXPECT_NEAR(-i * 1.0e-6, position.encoder, 1e-8);
- EXPECT_EQ(4, position.posedge_count);
- EXPECT_DOUBLE_EQ(0.75, position.posedge_value);
- EXPECT_EQ(4, position.negedge_count);
- EXPECT_DOUBLE_EQ(0.25, position.negedge_value);
+ position = sim.FillSensorValues<HallEffectAndPosition>(&fbb);
+ EXPECT_FALSE(position->current());
+ EXPECT_NEAR(-i * 1.0e-6, position->encoder(), 1e-8);
+ EXPECT_EQ(4, position->posedge_count());
+ EXPECT_DOUBLE_EQ(0.75, position->posedge_value());
+ EXPECT_EQ(4, position->negedge_count());
+ EXPECT_DOUBLE_EQ(0.25, position->negedge_value());
}
}
diff --git a/frc971/control_loops/profiled_subsystem.fbs b/frc971/control_loops/profiled_subsystem.fbs
new file mode 100644
index 0000000..46bc9fc
--- /dev/null
+++ b/frc971/control_loops/profiled_subsystem.fbs
@@ -0,0 +1,204 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace frc971.control_loops;
+
+table ProfiledJointStatus {
+ // Is the subsystem zeroed?
+ zeroed:bool;
+
+ // The state of the subsystem, if applicable. -1 otherwise.
+ // TODO(alex): replace with enum.
+ state:int;
+
+ // If true, we have aborted.
+ estopped:bool;
+
+ // Position of the joint.
+ position:float;
+ // Velocity of the joint in units/second.
+ velocity:float;
+ // Profiled goal position of the joint.
+ goal_position:float;
+ // Profiled goal velocity of the joint in units/second.
+ goal_velocity:float;
+ // Unprofiled goal position from absoulte zero of the joint.
+ unprofiled_goal_position:float;
+ // Unprofiled goal velocity of the joint in units/second.
+ unprofiled_goal_velocity:float;
+
+ // The estimated voltage error.
+ voltage_error:float;
+
+ // The calculated velocity with delta x/delta t
+ calculated_velocity:float;
+
+ // Components of the control loop output
+ position_power:float;
+ velocity_power:float;
+ feedforwards_power:float;
+
+ // State of the estimator.
+ estimator_state:frc971.EstimatorState;
+}
+
+table HallProfiledJointStatus {
+ // Is the subsystem zeroed?
+ zeroed:bool;
+
+ // The state of the subsystem, if applicable. -1 otherwise.
+ // TODO(alex): replace with enum.
+ state:int;
+
+ // If true, we have aborted.
+ estopped:bool;
+
+ // Position of the joint.
+ position:float;
+ // Velocity of the joint in units/second.
+ velocity:float;
+ // Profiled goal position of the joint.
+ goal_position:float;
+ // Profiled goal velocity of the joint in units/second.
+ goal_velocity:float;
+ // Unprofiled goal position from absoulte zero of the joint.
+ unprofiled_goal_position:float;
+ // Unprofiled goal velocity of the joint in units/second.
+ unprofiled_goal_velocity:float;
+
+ // The estimated voltage error.
+ voltage_error:float;
+
+ // The calculated velocity with delta x/delta t
+ calculated_velocity:float;
+
+ // Components of the control loop output
+ position_power:float;
+ velocity_power:float;
+ feedforwards_power:float;
+
+ // State of the estimator.
+ estimator_state:frc971.HallEffectAndPositionEstimatorState;
+}
+
+table PotAndAbsoluteEncoderProfiledJointStatus {
+ // Is the subsystem zeroed?
+ zeroed:bool;
+
+ // The state of the subsystem, if applicable. -1 otherwise.
+ // TODO(alex): replace with enum.
+ state:int;
+
+ // If true, we have aborted.
+ estopped:bool;
+
+ // Position of the joint.
+ position:float;
+ // Velocity of the joint in units/second.
+ velocity:float;
+ // Profiled goal position of the joint.
+ goal_position:float;
+ // Profiled goal velocity of the joint in units/second.
+ goal_velocity:float;
+ // Unprofiled goal position from absoulte zero of the joint.
+ unprofiled_goal_position:float;
+ // Unprofiled goal velocity of the joint in units/second.
+ unprofiled_goal_velocity:float;
+
+ // The estimated voltage error.
+ voltage_error:float;
+
+ // The calculated velocity with delta x/delta t
+ calculated_velocity:float;
+
+ // Components of the control loop output
+ position_power:float;
+ velocity_power:float;
+ feedforwards_power:float;
+
+ // State of the estimator.
+ estimator_state:frc971.PotAndAbsoluteEncoderEstimatorState;
+}
+
+table IndexProfiledJointStatus {
+ // Is the subsystem zeroed?
+ zeroed:bool;
+
+ // The state of the subsystem, if applicable. -1 otherwise.
+ // TODO(alex): replace with enum.
+ state:int;
+
+ // If true, we have aborted.
+ estopped:bool;
+
+ // Position of the joint.
+ position:float;
+ // Velocity of the joint in units/second.
+ velocity:float;
+ // Profiled goal position of the joint.
+ goal_position:float;
+ // Profiled goal velocity of the joint in units/second.
+ goal_velocity:float;
+ // Unprofiled goal position from absoulte zero of the joint.
+ unprofiled_goal_position:float;
+ // Unprofiled goal velocity of the joint in units/second.
+ unprofiled_goal_velocity:float;
+
+ // The estimated voltage error.
+ voltage_error:float;
+
+ // The calculated velocity with delta x/delta t
+ calculated_velocity:float;
+
+ // Components of the control loop output
+ position_power:float;
+ velocity_power:float;
+ feedforwards_power:float;
+
+ // State of the estimator.
+ estimator_state:frc971.IndexEstimatorState;
+}
+
+table AbsoluteEncoderProfiledJointStatus {
+ // Is the subsystem zeroed?
+ zeroed:bool;
+
+ // The state of the subsystem, if applicable. -1 otherwise.
+ // TODO(alex): replace with enum.
+ state:int;
+
+ // If true, we have aborted.
+ estopped:bool;
+
+ // Position of the joint.
+ position:float;
+ // Velocity of the joint in units/second.
+ velocity:float;
+ // Profiled goal position of the joint.
+ goal_position:float;
+ // Profiled goal velocity of the joint in units/second.
+ goal_velocity:float;
+ // Unprofiled goal position from absoulte zero of the joint.
+ unprofiled_goal_position:float;
+ // Unprofiled goal velocity of the joint in units/second.
+ unprofiled_goal_velocity:float;
+
+ // The estimated voltage error.
+ voltage_error:float;
+
+ // The calculated velocity with delta x/delta t
+ calculated_velocity:float;
+
+ // Components of the control loop output
+ position_power:float;
+ velocity_power:float;
+ feedforwards_power:float;
+
+ // State of the estimator.
+ estimator_state:frc971.AbsoluteEncoderEstimatorState;
+}
+
+table StaticZeroingSingleDOFProfiledSubsystemGoal {
+ unsafe_goal:double;
+
+ profile_params:frc971.ProfileParameters;
+}
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index 58058c7..2752dcc 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -11,8 +11,8 @@
#include "aos/controls/control_loop.h"
#include "aos/util/trapezoid_profile.h"
#include "frc971/constants.h"
-#include "frc971/control_loops/control_loops.q.h"
-#include "frc971/control_loops/profiled_subsystem.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
#include "frc971/control_loops/simple_capped_state_feedback_loop.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/zeroing/zeroing.h"
@@ -106,8 +106,9 @@
}
// Returns the current internal estimator state for logging.
- typename ZeroingEstimator::State EstimatorState(int index) {
- return estimators_[index].GetEstimatorState();
+ flatbuffers::Offset<typename ZeroingEstimator::State> EstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb, int index) {
+ return estimators_[index].GetEstimatorState(fbb);
}
// Sets the maximum voltage that will be commanded by the loop.
@@ -152,13 +153,14 @@
double default_angular_acceleration);
// Updates our estimator with the latest position.
- void Correct(typename ZeroingEstimator::Position position);
+ void Correct(const typename ZeroingEstimator::Position &position);
// Runs the controller and profile generator for a cycle.
void Update(bool disabled);
// Fills out the ProfiledJointStatus structure with the current state.
- template <class StatusType>
- void PopulateStatus(StatusType *status);
+ template <class StatusTypeBuilder>
+ StatusTypeBuilder BuildStatus(
+ flatbuffers::FlatBufferBuilder *fbb);
// Forces the current goal to the provided goal, bypassing the profiler.
void ForceGoal(double goal);
@@ -166,7 +168,7 @@
// this goal.
void set_unprofiled_goal(double unprofiled_goal);
// Limits our profiles to a max velocity and acceleration for proper motion.
- void AdjustProfile(const ::frc971::ProfileParameters &profile_parameters);
+ void AdjustProfile(const ::frc971::ProfileParameters *profile_parameters);
void AdjustProfile(double max_angular_velocity,
double max_angular_acceleration);
@@ -248,37 +250,41 @@
}
template <class ZeroingEstimator>
-template <class StatusType>
-void SingleDOFProfiledSubsystem<ZeroingEstimator>::PopulateStatus(
- StatusType *status) {
- status->zeroed = this->zeroed();
- status->state = -1;
+template <class StatusTypeBuilder>
+StatusTypeBuilder SingleDOFProfiledSubsystem<ZeroingEstimator>::BuildStatus(
+ flatbuffers::FlatBufferBuilder *fbb) {
+ flatbuffers::Offset<typename ZeroingEstimator::State> estimator_state =
+ this->EstimatorState(fbb, 0);
+
+ StatusTypeBuilder builder(*fbb);
+
+ builder.add_zeroed(this->zeroed());
// We don't know, so default to the bad case.
- status->estopped = true;
- status->position = this->X_hat(0, 0);
- status->velocity = this->X_hat(1, 0);
- status->goal_position = this->goal(0, 0);
- status->goal_velocity = this->goal(1, 0);
- status->unprofiled_goal_position = this->unprofiled_goal(0, 0);
- status->unprofiled_goal_velocity = this->unprofiled_goal(1, 0);
- status->voltage_error = this->X_hat(2, 0);
- status->calculated_velocity =
+ builder.add_position(this->X_hat(0, 0));
+ builder.add_velocity(this->X_hat(1, 0));
+ builder.add_goal_position(this->goal(0, 0));
+ builder.add_goal_velocity(this->goal(1, 0));
+ builder.add_unprofiled_goal_position(this->unprofiled_goal(0, 0));
+ builder.add_unprofiled_goal_velocity(this->unprofiled_goal(1, 0));
+ builder.add_voltage_error(this->X_hat(2, 0));
+ builder.add_calculated_velocity(
(position() - last_position_) /
- ::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency);
+ ::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency));
- status->estimator_state = this->EstimatorState(0);
+ builder.add_estimator_state(estimator_state);
Eigen::Matrix<double, 3, 1> error = this->controller().error();
- status->position_power =
- this->controller().controller().K(0, 0) * error(0, 0);
- status->velocity_power =
- this->controller().controller().K(0, 1) * error(1, 0);
+ builder.add_position_power(
+ this->controller().controller().K(0, 0) * error(0, 0));
+ builder.add_velocity_power(
+ this->controller().controller().K(0, 1) * error(1, 0));
+ return builder;
}
template <class ZeroingEstimator>
void SingleDOFProfiledSubsystem<ZeroingEstimator>::Correct(
- typename ZeroingEstimator::Position new_position) {
+ const typename ZeroingEstimator::Position &new_position) {
this->estimators_[0].UpdateEstimate(new_position);
if (this->estimators_[0].error()) {
@@ -299,7 +305,7 @@
}
last_position_ = position();
- this->Y_ << new_position.encoder;
+ this->Y_ << new_position.encoder();
this->Y_ += this->offset_;
this->loop_->Correct(Y_);
}
@@ -385,9 +391,11 @@
template <class ZeroingEstimator>
void SingleDOFProfiledSubsystem<ZeroingEstimator>::AdjustProfile(
- const ::frc971::ProfileParameters &profile_parameters) {
- AdjustProfile(profile_parameters.max_velocity,
- profile_parameters.max_acceleration);
+ const ::frc971::ProfileParameters *profile_parameters) {
+ AdjustProfile(
+ profile_parameters != nullptr ? profile_parameters->max_velocity() : 0.0,
+ profile_parameters != nullptr ? profile_parameters->max_acceleration()
+ : 0.0);
}
template <class ZeroingEstimator>
diff --git a/frc971/control_loops/profiled_subsystem.q b/frc971/control_loops/profiled_subsystem.q
deleted file mode 100644
index ac23907..0000000
--- a/frc971/control_loops/profiled_subsystem.q
+++ /dev/null
@@ -1,198 +0,0 @@
-package frc971.control_loops;
-
-import "frc971/control_loops/control_loops.q";
-
-struct ProfiledJointStatus {
- // Is the subsystem zeroed?
- bool zeroed;
-
- // The state of the subsystem, if applicable. -1 otherwise.
- int32_t state;
-
- // If true, we have aborted.
- bool estopped;
-
- // Position of the joint.
- float position;
- // Velocity of the joint in units/second.
- float velocity;
- // Profiled goal position of the joint.
- float goal_position;
- // Profiled goal velocity of the joint in units/second.
- float goal_velocity;
- // Unprofiled goal position from absoulte zero of the joint.
- float unprofiled_goal_position;
- // Unprofiled goal velocity of the joint in units/second.
- float unprofiled_goal_velocity;
-
- // The estimated voltage error.
- float voltage_error;
-
- // The calculated velocity with delta x/delta t
- float calculated_velocity;
-
- // Components of the control loop output
- float position_power;
- float velocity_power;
- float feedforwards_power;
-
- // State of the estimator.
- .frc971.EstimatorState estimator_state;
-};
-
-struct HallProfiledJointStatus {
- // Is the subsystem zeroed?
- bool zeroed;
-
- // The state of the subsystem, if applicable. -1 otherwise.
- int32_t state;
-
- // If true, we have aborted.
- bool estopped;
-
- // Position of the joint.
- float position;
- // Velocity of the joint in units/second.
- float velocity;
- // Profiled goal position of the joint.
- float goal_position;
- // Profiled goal velocity of the joint in units/second.
- float goal_velocity;
- // Unprofiled goal position from absoulte zero of the joint.
- float unprofiled_goal_position;
- // Unprofiled goal velocity of the joint in units/second.
- float unprofiled_goal_velocity;
-
- // The estimated voltage error.
- float voltage_error;
-
- // The calculated velocity with delta x/delta t
- float calculated_velocity;
-
- // Components of the control loop output
- float position_power;
- float velocity_power;
- float feedforwards_power;
-
- // State of the estimator.
- .frc971.HallEffectAndPositionEstimatorState estimator_state;
-};
-
-struct PotAndAbsoluteEncoderProfiledJointStatus {
- // Is the subsystem zeroed?
- bool zeroed;
-
- // The state of the subsystem, if applicable. -1 otherwise.
- int32_t state;
-
- // If true, we have aborted.
- bool estopped;
-
- // Position of the joint.
- float position;
- // Velocity of the joint in units/second.
- float velocity;
- // Profiled goal position of the joint.
- float goal_position;
- // Profiled goal velocity of the joint in units/second.
- float goal_velocity;
- // Unprofiled goal position from absoulte zero of the joint.
- float unprofiled_goal_position;
- // Unprofiled goal velocity of the joint in units/second.
- float unprofiled_goal_velocity;
-
- // The estimated voltage error.
- float voltage_error;
-
- // The calculated velocity with delta x/delta t
- float calculated_velocity;
-
- // Components of the control loop output
- float position_power;
- float velocity_power;
- float feedforwards_power;
-
- // State of the estimator.
- .frc971.PotAndAbsoluteEncoderEstimatorState estimator_state;
-};
-
-struct IndexProfiledJointStatus {
- // Is the subsystem zeroed?
- bool zeroed;
-
- // The state of the subsystem, if applicable. -1 otherwise.
- int32_t state;
-
- // If true, we have aborted.
- bool estopped;
-
- // Position of the joint.
- float position;
- // Velocity of the joint in units/second.
- float velocity;
- // Profiled goal position of the joint.
- float goal_position;
- // Profiled goal velocity of the joint in units/second.
- float goal_velocity;
- // Unprofiled goal position from absoulte zero of the joint.
- float unprofiled_goal_position;
- // Unprofiled goal velocity of the joint in units/second.
- float unprofiled_goal_velocity;
-
- // The estimated voltage error.
- float voltage_error;
-
- // The calculated velocity with delta x/delta t
- float calculated_velocity;
-
- // Components of the control loop output
- float position_power;
- float velocity_power;
- float feedforwards_power;
-
- // State of the estimator.
- .frc971.IndexEstimatorState estimator_state;
-};
-
-struct AbsoluteEncoderProfiledJointStatus {
- // Is the subsystem zeroed?
- bool zeroed;
-
- // The state of the subsystem, if applicable. -1 otherwise.
- int32_t state;
-
- // If true, we have aborted.
- bool estopped;
-
- // Position of the joint.
- float position;
- // Velocity of the joint in units/second.
- float velocity;
- // Profiled goal position of the joint.
- float goal_position;
- // Profiled goal velocity of the joint in units/second.
- float goal_velocity;
- // Unprofiled goal position from absoulte zero of the joint.
- float unprofiled_goal_position;
- // Unprofiled goal velocity of the joint in units/second.
- float unprofiled_goal_velocity;
-
- // The estimated voltage error.
- float voltage_error;
-
- // The calculated velocity with delta x/delta t
- float calculated_velocity;
-
- // Components of the control loop output
- float position_power;
- float velocity_power;
- float feedforwards_power;
-
- // State of the estimator.
- .frc971.AbsoluteEncoderEstimatorState estimator_state;
-};
-
-struct StaticZeroingSingleDOFProfiledSubsystemGoal {
- double unsafe_goal;
- .frc971.ProfileParameters profile_params;
-};
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
index 748086f..b220581 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
@@ -6,6 +6,12 @@
namespace frc971 {
namespace control_loops {
+// TODO(austin): Use ProfileParametersT...
+struct ProfileParametersStruct {
+ float max_velocity;
+ float max_acceleration;
+};
+
template <typename ZeroingEstimator>
struct StaticZeroingSingleDOFProfiledSubsystemParams {
// Maximum voltage while the subsystem is zeroing
@@ -15,11 +21,11 @@
double operating_voltage;
// Maximum velocity (units/s) and acceleration while State::ZEROING
- ::frc971::ProfileParameters zeroing_profile_params;
+ ProfileParametersStruct zeroing_profile_params;
// Maximum velocity (units/s) and acceleration while State::RUNNING if max
// velocity or acceleration in goal profile_params is 0
- ::frc971::ProfileParameters default_profile_params;
+ ProfileParametersStruct default_profile_params;
// Maximum range of the subsystem in meters
::frc971::constants::Range range;
@@ -65,9 +71,10 @@
// Returns the current position
double position() const { return profiled_subsystem_.position(); }
- void Iterate(const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
- const typename ZeroingEstimator::Position *position,
- double *output, ProfiledJointStatus *status);
+ flatbuffers::Offset<ProfiledJointStatus> Iterate(
+ const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
+ const typename ZeroingEstimator::Position *position, double *output,
+ flatbuffers::FlatBufferBuilder *status_fbb);
// Resets the profiled subsystem and returns to uninitialized
void Reset();
@@ -125,11 +132,11 @@
}
template <typename ZeroingEstimator, typename ProfiledJointStatus>
-void StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator,
- ProfiledJointStatus>::
+flatbuffers::Offset<ProfiledJointStatus>
+StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator, ProfiledJointStatus>::
Iterate(const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
const typename ZeroingEstimator::Position *position, double *output,
- ProfiledJointStatus *status) {
+ flatbuffers::FlatBufferBuilder *status_fbb) {
bool disabled = output == nullptr;
profiled_subsystem_.Correct(*position);
@@ -159,7 +166,9 @@
// jump.
profiled_subsystem_.ForceGoal(profiled_subsystem_.position());
// Set up the profile to be the zeroing profile.
- profiled_subsystem_.AdjustProfile(params_.zeroing_profile_params);
+ profiled_subsystem_.AdjustProfile(
+ params_.zeroing_profile_params.max_velocity,
+ params_.zeroing_profile_params.max_acceleration);
// We are not ready to start doing anything yet.
disabled = true;
@@ -181,9 +190,9 @@
}
if (goal) {
- profiled_subsystem_.AdjustProfile(goal->profile_params);
+ profiled_subsystem_.AdjustProfile(goal->profile_params());
- double safe_goal = goal->unsafe_goal;
+ double safe_goal = goal->unsafe_goal();
if (safe_goal < min_position_) {
AOS_LOG(DEBUG, "Limiting to %f from %f\n", min_position_, safe_goal);
safe_goal = min_position_;
@@ -217,11 +226,14 @@
*output = profiled_subsystem_.voltage();
}
- status->zeroed = profiled_subsystem_.zeroed();
+ typename ProfiledJointStatus::Builder status_builder =
+ profiled_subsystem_
+ .template BuildStatus<typename ProfiledJointStatus::Builder>(
+ status_fbb);
- profiled_subsystem_.PopulateStatus(status);
- status->estopped = (state_ == State::ESTOP);
- status->state = static_cast<int32_t>(state_);
+ status_builder.add_estopped(state_ == State::ESTOP);
+ status_builder.add_state(static_cast<int32_t>(state_));
+ return status_builder.Finish();
}
} // namespace control_loops
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
index 96cb782..46e9c76 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
@@ -1,3 +1,4 @@
+#include "flatbuffers/flatbuffers.h"
#include "gtest/gtest.h"
#include "aos/controls/control_loop.h"
@@ -5,9 +6,10 @@
#include "frc971/control_loops/capped_test_plant.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
-#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.q.h"
+#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_generated.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_integral_plant.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test_plant.h"
+#include "frc971/zeroing/zeroing.h"
using ::frc971::control_loops::PositionSensorSimulator;
@@ -20,20 +22,32 @@
using ::aos::monotonic_clock;
using SZSDPS_PotAndAbsEncoder = StaticZeroingSingleDOFProfiledSubsystem<
- zeroing::PotAndAbsoluteEncoderZeroingEstimator,
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
using SZSDPS_AbsEncoder = StaticZeroingSingleDOFProfiledSubsystem<
- zeroing::AbsoluteEncoderZeroingEstimator,
+ ::frc971::zeroing::AbsoluteEncoderZeroingEstimator,
::frc971::control_loops::AbsoluteEncoderProfiledJointStatus>;
+using FBB = flatbuffers::FlatBufferBuilder;
+
+struct PotAndAbsoluteEncoderQueueGroup {
+ typedef PotAndAbsoluteEncoderProfiledJointStatus Status;
+ typedef PotAndAbsolutePosition Position;
+ typedef ::frc971::control_loops::zeroing::testing::SubsystemGoal Goal;
+ typedef ::frc971::control_loops::zeroing::testing::SubsystemOutput Output;
+};
+
+struct AbsoluteEncoderQueueGroup {
+ typedef AbsoluteEncoderProfiledJointStatus Status;
+ typedef AbsolutePosition Position;
+ typedef zeroing::testing::SubsystemGoal Goal;
+ typedef zeroing::testing::SubsystemOutput Output;
+};
+
typedef ::testing::Types<
- ::std::pair<
- SZSDPS_AbsEncoder,
- StaticZeroingSingleDOFProfiledSubsystemAbsoluteEncoderTestQueueGroup>,
- ::std::pair<
- SZSDPS_PotAndAbsEncoder,
- StaticZeroingSingleDOFProfiledSubsystemPotAndAbsoluteEncoderTestQueueGroup>>
+ ::std::pair<SZSDPS_AbsEncoder, AbsoluteEncoderQueueGroup>,
+ ::std::pair<SZSDPS_PotAndAbsEncoder, PotAndAbsoluteEncoderQueueGroup>>
TestTypes;
constexpr ::frc971::constants::Range kRange{
@@ -54,14 +68,15 @@
};
template <>
-const zeroing::PotAndAbsoluteEncoderZeroingEstimator::ZeroingConstants
+const frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator::ZeroingConstants
TestIntakeSystemValues<
- zeroing::PotAndAbsoluteEncoderZeroingEstimator>::kZeroing{
+ frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>::kZeroing{
kZeroingSampleSize, kEncoderIndexDifference, 0, 0.0005, 20, 1.9};
template <>
-const zeroing::AbsoluteEncoderZeroingEstimator::ZeroingConstants
- TestIntakeSystemValues<zeroing::AbsoluteEncoderZeroingEstimator>::kZeroing{
+const frc971::zeroing::AbsoluteEncoderZeroingEstimator::ZeroingConstants
+ TestIntakeSystemValues<
+ frc971::zeroing::AbsoluteEncoderZeroingEstimator>::kZeroing{
kZeroingSampleSize, kEncoderIndexDifference, 0.0, 0.2, 0.0005, 20, 1.9};
template <typename ZeroingEstimator>
@@ -83,29 +98,21 @@
template <typename SZSDPS, typename QueueGroup>
class TestIntakeSystemSimulation {
public:
- typedef typename std::remove_reference<decltype(
- *(static_cast<QueueGroup *>(NULL)->goal.MakeMessage().get()))>::type
- GoalType;
- typedef typename std::remove_reference<decltype(
- *(static_cast<QueueGroup *>(NULL)->position.MakeMessage().get()))>::type
- PositionType;
- typedef typename std::remove_reference<decltype(
- *(static_cast<QueueGroup *>(NULL)->status.MakeMessage().get()))>::type
- StatusType;
- typedef typename std::remove_reference<decltype(
- *(static_cast<QueueGroup *>(NULL)->output.MakeMessage().get()))>::type
- OutputType;
+ typedef typename QueueGroup::Goal GoalType;
+ typedef typename QueueGroup::Status StatusType;
+ typedef typename QueueGroup::Position PositionType;
+ typedef typename QueueGroup::Output OutputType;
TestIntakeSystemSimulation(::aos::EventLoop *event_loop,
chrono::nanoseconds dt)
: event_loop_(event_loop),
dt_(dt),
subsystem_position_sender_(
- event_loop_->MakeSender<PositionType>(".position")),
+ event_loop_->MakeSender<PositionType>("/loop")),
subsystem_status_fetcher_(
- event_loop_->MakeFetcher<StatusType>(".status")),
+ event_loop_->MakeFetcher<StatusType>("/loop")),
subsystem_output_fetcher_(
- event_loop_->MakeFetcher<OutputType>(".output")),
+ event_loop_->MakeFetcher<OutputType>("/loop")),
subsystem_plant_(new CappedTestPlant(
::frc971::control_loops::MakeTestIntakeSystemPlant())),
subsystem_sensor_sim_(kEncoderIndexDifference) {
@@ -133,8 +140,8 @@
void InitializeSensorSim(double start_pos);
- double subsystem_position() const { return this->subsystem_plant_->X(0, 0); }
- double subsystem_velocity() const { return this->subsystem_plant_->X(1, 0); }
+ double subsystem_position() const { return subsystem_plant_->X(0, 0); }
+ double subsystem_velocity() const { return subsystem_plant_->X(1, 0); }
// Sets the difference between the commanded and applied powers.
// This lets us test that the integrators work.
@@ -144,12 +151,13 @@
// Sends a queue message with the position.
void SendPositionMessage() {
- typename ::aos::Sender<PositionType>::Message position =
- subsystem_position_sender_.MakeMessage();
+ typename ::aos::Sender<PositionType>::Builder position =
+ subsystem_position_sender_.MakeBuilder();
- this->subsystem_sensor_sim_.GetSensorValues(&position->position);
-
- position.Send();
+ auto position_builder = position.template MakeBuilder<PositionType>();
+ position.Send(this->subsystem_sensor_sim_
+ .template GetSensorValues<typename PositionType::Builder>(
+ &position_builder));
}
void set_peak_subsystem_acceleration(double value) {
@@ -168,15 +176,15 @@
const double voltage_check_subsystem =
(static_cast<typename SZSDPS::State>(
- subsystem_status_fetcher_->status.state) == SZSDPS::State::RUNNING)
+ subsystem_status_fetcher_->state()) == SZSDPS::State::RUNNING)
? kOperatingVoltage
: kZeroingVoltage;
- EXPECT_LE(::std::abs(subsystem_output_fetcher_->output),
+ EXPECT_LE(::std::abs(subsystem_output_fetcher_->output()),
voltage_check_subsystem);
::Eigen::Matrix<double, 1, 1> subsystem_U;
- subsystem_U << subsystem_output_fetcher_->output +
+ subsystem_U << subsystem_output_fetcher_->output() +
subsystem_plant_->voltage_offset();
subsystem_plant_->Update(subsystem_U);
@@ -211,13 +219,14 @@
double peak_subsystem_acceleration_ = 1e10;
// The velocity limits to check for while moving.
double peak_subsystem_velocity_ = 1e10;
+
+ flatbuffers::FlatBufferBuilder fbb;
};
template <>
void TestIntakeSystemSimulation<
SZSDPS_PotAndAbsEncoder,
- StaticZeroingSingleDOFProfiledSubsystemPotAndAbsoluteEncoderTestQueueGroup>::
- InitializeSensorSim(double start_pos) {
+ PotAndAbsoluteEncoderQueueGroup>::InitializeSensorSim(double start_pos) {
subsystem_sensor_sim_.Initialize(
start_pos, kNoiseScalar, 0.0,
TestIntakeSystemValues<
@@ -226,9 +235,7 @@
}
template <>
-void TestIntakeSystemSimulation<
- SZSDPS_AbsEncoder,
- StaticZeroingSingleDOFProfiledSubsystemAbsoluteEncoderTestQueueGroup>::
+void TestIntakeSystemSimulation<SZSDPS_AbsEncoder, AbsoluteEncoderQueueGroup>::
InitializeSensorSim(double start_pos) {
subsystem_sensor_sim_.Initialize(
start_pos, kNoiseScalar, 0.0,
@@ -240,46 +247,73 @@
// Class to represent a module using a subsystem. This lets us use event loops
// to wrap it.
template <typename QueueGroup, typename SZSDPS>
-class Subsystem : public ::aos::controls::ControlLoop<QueueGroup> {
+class Subsystem
+ : public ::aos::controls::ControlLoop<
+ typename QueueGroup::Goal, typename QueueGroup::Position,
+ typename QueueGroup::Status, typename QueueGroup::Output> {
public:
- typedef typename std::remove_reference<decltype(
- *(static_cast<QueueGroup *>(NULL)->goal.MakeMessage().get()))>::type
- GoalType;
- typedef typename std::remove_reference<decltype(
- *(static_cast<QueueGroup *>(NULL)->position.MakeMessage().get()))>::type
- PositionType;
- typedef typename std::remove_reference<decltype(
- *(static_cast<QueueGroup *>(NULL)->status.MakeMessage().get()))>::type
- StatusType;
- typedef typename std::remove_reference<decltype(
- *(static_cast<QueueGroup *>(NULL)->output.MakeMessage().get()))>::type
- OutputType;
+ typedef typename QueueGroup::Goal GoalType;
+ typedef typename QueueGroup::Status StatusType;
+ typedef typename QueueGroup::Position PositionType;
+ typedef typename QueueGroup::Output OutputType;
Subsystem(::aos::EventLoop *event_loop, const ::std::string &name)
- : aos::controls::ControlLoop<QueueGroup>(event_loop, name),
+ : aos::controls::ControlLoop<
+ typename QueueGroup::Goal, typename QueueGroup::Position,
+ typename QueueGroup::Status, typename QueueGroup::Output>(
+ event_loop, name),
subsystem_(TestIntakeSystemValues<
typename SZSDPS::ZeroingEstimator>::make_params()) {}
void RunIteration(const GoalType *unsafe_goal, const PositionType *position,
- OutputType *output, StatusType *status) {
+ typename ::aos::Sender<OutputType>::Builder *output,
+ typename ::aos::Sender<StatusType>::Builder *status) {
if (this->WasReset()) {
AOS_LOG(ERROR, "WPILib reset, restarting\n");
subsystem_.Reset();
}
// Convert one goal type to another...
- StaticZeroingSingleDOFProfiledSubsystemGoal goal;
+ // TODO(austin): This mallocs...
+ FBB fbb;
+ ProfileParametersBuilder params_builder(fbb);
if (unsafe_goal != nullptr ) {
- goal.unsafe_goal = unsafe_goal->unsafe_goal;
- goal.profile_params.max_velocity =
- unsafe_goal->profile_params.max_velocity;
- goal.profile_params.max_acceleration =
- unsafe_goal->profile_params.max_acceleration;
+ if (unsafe_goal->profile_params() != nullptr) {
+ params_builder.add_max_velocity(
+ unsafe_goal->profile_params()->max_velocity());
+ params_builder.add_max_acceleration(
+ unsafe_goal->profile_params()->max_acceleration());
+ }
+
+ const auto params_builder_offset = params_builder.Finish();
+ StaticZeroingSingleDOFProfiledSubsystemGoalBuilder goal_builder(fbb);
+ goal_builder.add_unsafe_goal(unsafe_goal->unsafe_goal());
+ goal_builder.add_profile_params(params_builder_offset);
+ fbb.Finish(goal_builder.Finish());
+ } else {
+ params_builder.add_max_velocity(0.0);
+ params_builder.add_max_acceleration(0.0);
+ const auto params_builder_offset = params_builder.Finish();
+ StaticZeroingSingleDOFProfiledSubsystemGoalBuilder goal_builder(fbb);
+ goal_builder.add_profile_params(params_builder_offset);
+ fbb.Finish(goal_builder.Finish());
}
- subsystem_.Iterate(
- unsafe_goal == nullptr ? nullptr : &goal, &position->position,
- output == nullptr ? nullptr : &output->output, &status->status);
+ double output_voltage;
+
+ flatbuffers::Offset<StatusType> status_offset = subsystem_.Iterate(
+ unsafe_goal == nullptr
+ ? nullptr
+ : flatbuffers::GetRoot<StaticZeroingSingleDOFProfiledSubsystemGoal>(
+ fbb.GetBufferPointer()),
+ position, output == nullptr ? nullptr : &output_voltage, status->fbb());
+ status->Send(status_offset);
+ if (output != nullptr) {
+ typename OutputType::Builder output_builder =
+ output->template MakeBuilder<OutputType>();
+ output_builder.add_output(output_voltage);
+ output->Send(output_builder.Finish());
+ }
}
SZSDPS *subsystem() { return &subsystem_; }
@@ -296,29 +330,58 @@
using ZeroingEstimator = typename SZSDPS::ZeroingEstimator;
using ProfiledJointStatus = typename SZSDPS::ProfiledJointStatus;
- typedef typename std::remove_reference<decltype(
- *(static_cast<QueueGroup *>(NULL)->goal.MakeMessage().get()))>::type
- GoalType;
- typedef typename std::remove_reference<decltype(
- *(static_cast<QueueGroup *>(NULL)->position.MakeMessage().get()))>::type
- PositionType;
- typedef typename std::remove_reference<decltype(
- *(static_cast<QueueGroup *>(NULL)->status.MakeMessage().get()))>::type
- StatusType;
- typedef typename std::remove_reference<decltype(
- *(static_cast<QueueGroup *>(NULL)->output.MakeMessage().get()))>::type
- OutputType;
+ typedef typename QueueGroup::Goal GoalType;
+ typedef typename QueueGroup::Status StatusType;
+ typedef typename QueueGroup::Position PositionType;
+ typedef typename QueueGroup::Output OutputType;
IntakeSystemTest()
- : ::aos::testing::ControlLoopTest(chrono::microseconds(5050)),
+ : ::aos::testing::ControlLoopTest(
+ "{\n"
+ " \"channels\": [ \n"
+ " {\n"
+ " \"name\": \"/aos\",\n"
+ " \"type\": \"aos.JoystickState\"\n"
+ " },\n"
+ " {\n"
+ " \"name\": \"/aos\",\n"
+ " \"type\": \"aos.RobotState\"\n"
+ " },\n"
+ " {\n"
+ " \"name\": \"/loop\",\n"
+ " \"type\": \"frc971.control_loops.zeroing.testing.SubsystemGoal\"\n"
+ " },\n"
+ " {\n"
+ " \"name\": \"/loop\",\n"
+ " \"type\": \"frc971.control_loops.zeroing.testing.SubsystemOutput\"\n"
+ " },\n"
+ " {\n"
+ " \"name\": \"/loop\",\n"
+ " \"type\": \"frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus\"\n"
+ " },\n"
+ " {\n"
+ " \"name\": \"/loop\",\n"
+ " \"type\": \"frc971.AbsolutePosition\"\n"
+ " },\n"
+ " {\n"
+ " \"name\": \"/loop\",\n"
+ " \"type\": \"frc971.PotAndAbsolutePosition\"\n"
+ " },\n"
+ " {\n"
+ " \"name\": \"/loop\",\n"
+ " \"type\": \"frc971.control_loops.AbsoluteEncoderProfiledJointStatus\"\n"
+ " }\n"
+ " ]\n"
+ "}\n",
+ chrono::microseconds(5050)),
test_event_loop_(MakeEventLoop()),
- subsystem_goal_sender_(test_event_loop_->MakeSender<GoalType>(".goal")),
+ subsystem_goal_sender_(test_event_loop_->MakeSender<GoalType>("/loop")),
subsystem_goal_fetcher_(
- test_event_loop_->MakeFetcher<GoalType>(".goal")),
+ test_event_loop_->MakeFetcher<GoalType>("/loop")),
subsystem_status_fetcher_(
- test_event_loop_->MakeFetcher<StatusType>(".status")),
+ test_event_loop_->MakeFetcher<StatusType>("/loop")),
subsystem_event_loop_(MakeEventLoop()),
- subsystem_(subsystem_event_loop_.get(), ""),
+ subsystem_(subsystem_event_loop_.get(), "/loop"),
subsystem_plant_event_loop_(MakeEventLoop()),
subsystem_plant_(subsystem_plant_event_loop_.get(), dt()) {}
@@ -327,20 +390,20 @@
EXPECT_TRUE(subsystem_goal_fetcher_.get() != nullptr);
EXPECT_TRUE(subsystem_status_fetcher_.Fetch());
- EXPECT_NEAR(subsystem_goal_fetcher_->unsafe_goal,
- subsystem_status_fetcher_->status.position, 0.001);
- EXPECT_NEAR(subsystem_goal_fetcher_->unsafe_goal,
+ EXPECT_NEAR(subsystem_goal_fetcher_->unsafe_goal(),
+ subsystem_status_fetcher_->position(), 0.001);
+ EXPECT_NEAR(subsystem_goal_fetcher_->unsafe_goal(),
subsystem_plant_.subsystem_position(), 0.001);
- EXPECT_NEAR(subsystem_status_fetcher_->status.velocity, 0, 0.001);
+ EXPECT_NEAR(subsystem_status_fetcher_->velocity(), 0, 0.001);
}
SZSDPS *subsystem() { return subsystem_.subsystem(); }
void set_peak_subsystem_acceleration(double value) {
- set_peak_subsystem_acceleration(value);
+ subsystem_plant_.set_peak_subsystem_acceleration(value);
}
void set_peak_subsystem_velocity(double value) {
- set_peak_subsystem_velocity(value);
+ subsystem_plant_.set_peak_subsystem_velocity(value);
}
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
@@ -363,9 +426,9 @@
this->SetEnabled(true);
// Intake system uses 0.05 to test for 0.
{
- auto message = this->subsystem_goal_sender_.MakeMessage();
- message->unsafe_goal = 0.05;
- EXPECT_TRUE(message.Send());
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ EXPECT_TRUE(message.Send(
+ zeroing::testing::CreateSubsystemGoal(*message.fbb(), 0.05)));
}
this->RunFor(chrono::seconds(5));
@@ -377,11 +440,13 @@
this->SetEnabled(true);
// Set a reasonable goal.
{
- auto message = this->subsystem_goal_sender_.MakeMessage();
- message->unsafe_goal = 0.1;
- message->profile_params.max_velocity = 1;
- message->profile_params.max_acceleration = 0.5;
- EXPECT_TRUE(message.Send());
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ auto profile_builder =
+ message.template MakeBuilder<frc971::ProfileParameters>();
+ profile_builder.add_max_velocity(1);
+ profile_builder.add_max_acceleration(0.5);
+ EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), 0.10, profile_builder.Finish())));
}
// Give it a lot of time to get there.
@@ -396,9 +461,9 @@
this->SetEnabled(true);
// Zero it before we move.
{
- auto message = this->subsystem_goal_sender_.MakeMessage();
- message->unsafe_goal = kRange.upper;
- EXPECT_TRUE(message.Send());
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ EXPECT_TRUE(message.Send(
+ zeroing::testing::CreateSubsystemGoal(*message.fbb(), kRange.upper)));
}
this->RunFor(chrono::seconds(8));
this->VerifyNearGoal();
@@ -406,11 +471,12 @@
// Try a low acceleration move with a high max velocity and verify the
// acceleration is capped like expected.
{
- auto message = this->subsystem_goal_sender_.MakeMessage();
- message->unsafe_goal = kRange.lower;
- message->profile_params.max_velocity = 20.0;
- message->profile_params.max_acceleration = 0.1;
- EXPECT_TRUE(message.Send());
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ auto profile_builder = message.template MakeBuilder<frc971::ProfileParameters>();
+ profile_builder.add_max_velocity(20.0);
+ profile_builder.add_max_acceleration(0.1);
+ EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), kRange.lower, profile_builder.Finish())));
}
this->set_peak_subsystem_velocity(23.0);
this->set_peak_subsystem_acceleration(0.2);
@@ -420,11 +486,13 @@
// Now do a high acceleration move with a low velocity limit.
{
- auto message = this->subsystem_goal_sender_.MakeMessage();
- message->unsafe_goal = kRange.upper;
- message->profile_params.max_velocity = 0.1;
- message->profile_params.max_acceleration = 100;
- EXPECT_TRUE(message.Send());
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ auto profile_builder =
+ message.template MakeBuilder<frc971::ProfileParameters>();
+ profile_builder.add_max_velocity(0.1);
+ profile_builder.add_max_acceleration(100.0);
+ EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), kRange.upper, profile_builder.Finish())));
}
this->set_peak_subsystem_velocity(0.2);
@@ -441,34 +509,34 @@
// Set some ridiculous goals to test upper limits.
{
- auto message = this->subsystem_goal_sender_.MakeMessage();
- message->unsafe_goal = 100.0;
- message->profile_params.max_velocity = 1;
- message->profile_params.max_acceleration = 0.5;
- EXPECT_TRUE(message.Send());
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ auto profile_builder = message.template MakeBuilder<frc971::ProfileParameters>();
+ profile_builder.add_max_velocity(1.0);
+ profile_builder.add_max_acceleration(0.5);
+ EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), 100.0, profile_builder.Finish())));
}
this->RunFor(chrono::seconds(10));
// Check that we are near our soft limit.
EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
- EXPECT_NEAR(kRange.upper, this->subsystem_status_fetcher_->status.position,
- 0.001);
+ EXPECT_NEAR(kRange.upper, this->subsystem_status_fetcher_->position(), 0.001);
// Set some ridiculous goals to test lower limits.
{
- auto message = this->subsystem_goal_sender_.MakeMessage();
- message->unsafe_goal = -100.0;
- message->profile_params.max_velocity = 1;
- message->profile_params.max_acceleration = 0.5;
- EXPECT_TRUE(message.Send());
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ auto profile_builder = message.template MakeBuilder<frc971::ProfileParameters>();
+ profile_builder.add_max_velocity(1.0);
+ profile_builder.add_max_acceleration(0.5);
+ EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), -100.0, profile_builder.Finish())));
}
this->RunFor(chrono::seconds(10));
// Check that we are near our soft limit.
EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
- EXPECT_NEAR(kRange.lower, this->subsystem_status_fetcher_->status.position,
- 0.001);
+ EXPECT_NEAR(kRange.lower, this->subsystem_status_fetcher_->position(), 0.001);
}
// Tests that the subsystem loop zeroes when run for a while.
@@ -476,11 +544,12 @@
this->SetEnabled(true);
{
- auto message = this->subsystem_goal_sender_.MakeMessage();
- message->unsafe_goal = kRange.upper;
- message->profile_params.max_velocity = 1;
- message->profile_params.max_acceleration = 0.5;
- EXPECT_TRUE(message.Send());
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ auto profile_builder = message.template MakeBuilder<frc971::ProfileParameters>();
+ profile_builder.add_max_velocity(1.0);
+ profile_builder.add_max_acceleration(0.5);
+ EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), kRange.upper, profile_builder.Finish())));
}
this->RunFor(chrono::seconds(10));
@@ -500,9 +569,9 @@
this->SetEnabled(true);
this->subsystem_plant_.InitializeSubsystemPosition(kRange.lower_hard);
{
- auto message = this->subsystem_goal_sender_.MakeMessage();
- message->unsafe_goal = kRange.upper;
- EXPECT_TRUE(message.Send());
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ EXPECT_TRUE(message.Send(
+ zeroing::testing::CreateSubsystemGoal(*message.fbb(), kRange.upper)));
}
this->RunFor(chrono::seconds(10));
@@ -515,9 +584,9 @@
this->subsystem_plant_.InitializeSubsystemPosition(kRange.upper_hard);
{
- auto message = this->subsystem_goal_sender_.MakeMessage();
- message->unsafe_goal = kRange.upper;
- EXPECT_TRUE(message.Send());
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ EXPECT_TRUE(message.Send(
+ zeroing::testing::CreateSubsystemGoal(*message.fbb(), kRange.upper)));
}
this->RunFor(chrono::seconds(10));
@@ -530,9 +599,9 @@
this->subsystem_plant_.InitializeSubsystemPosition(kRange.upper);
{
- auto message = this->subsystem_goal_sender_.MakeMessage();
- message->unsafe_goal = kRange.upper - 0.1;
- EXPECT_TRUE(message.Send());
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), kRange.upper - 0.1)));
}
this->RunFor(chrono::seconds(10));
@@ -554,9 +623,9 @@
// Tests that the internal goals don't change while disabled.
TYPED_TEST_P(IntakeSystemTest, DisabledGoalTest) {
{
- auto message = this->subsystem_goal_sender_.MakeMessage();
- message->unsafe_goal = kRange.lower + 0.03;
- EXPECT_TRUE(message.Send());
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), kRange.lower + 0.03)));
}
// Checks that the subsystem has not moved from its starting position at 0
@@ -572,9 +641,9 @@
// Tests that zeroing while disabled works.
TYPED_TEST_P(IntakeSystemTest, DisabledZeroTest) {
{
- auto message = this->subsystem_goal_sender_.MakeMessage();
- message->unsafe_goal = kRange.lower;
- EXPECT_TRUE(message.Send());
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ EXPECT_TRUE(message.Send(
+ zeroing::testing::CreateSubsystemGoal(*message.fbb(), kRange.lower)));
}
// Run disabled for 2 seconds
@@ -591,16 +660,16 @@
TYPED_TEST_P(IntakeSystemTest, MinPositionTest) {
this->SetEnabled(true);
{
- auto message = this->subsystem_goal_sender_.MakeMessage();
- message->unsafe_goal = kRange.lower_hard;
- EXPECT_TRUE(message.Send());
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ EXPECT_TRUE(message.Send(
+ zeroing::testing::CreateSubsystemGoal(*message.fbb(), kRange.lower_hard)));
}
this->RunFor(chrono::seconds(2));
// Check that kRange.lower is used as the default min position
EXPECT_EQ(this->subsystem()->goal(0), kRange.lower);
EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
- EXPECT_NEAR(kRange.lower, this->subsystem_status_fetcher_->status.position,
+ EXPECT_NEAR(kRange.lower, this->subsystem_status_fetcher_->position(),
0.001);
// Set min position and check that the subsystem increases to that position
@@ -608,7 +677,7 @@
this->RunFor(chrono::seconds(2));
EXPECT_EQ(this->subsystem()->goal(0), kRange.lower + 0.05);
EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
- EXPECT_NEAR(kRange.lower + 0.05, this->subsystem_status_fetcher_->status.position,
+ EXPECT_NEAR(kRange.lower + 0.05, this->subsystem_status_fetcher_->position(),
0.001);
// Clear min position and check that the subsystem returns to kRange.lower
@@ -616,7 +685,7 @@
this->RunFor(chrono::seconds(2));
EXPECT_EQ(this->subsystem()->goal(0), kRange.lower);
EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
- EXPECT_NEAR(kRange.lower, this->subsystem_status_fetcher_->status.position,
+ EXPECT_NEAR(kRange.lower, this->subsystem_status_fetcher_->position(),
0.001);
}
@@ -625,16 +694,16 @@
this->SetEnabled(true);
{
- auto message = this->subsystem_goal_sender_.MakeMessage();
- message->unsafe_goal = kRange.upper_hard;
- EXPECT_TRUE(message.Send());
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), kRange.upper_hard)));
}
this->RunFor(chrono::seconds(2));
// Check that kRange.upper is used as the default max position
EXPECT_EQ(this->subsystem()->goal(0), kRange.upper);
EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
- EXPECT_NEAR(kRange.upper, this->subsystem_status_fetcher_->status.position,
+ EXPECT_NEAR(kRange.upper, this->subsystem_status_fetcher_->position(),
0.001);
// Set max position and check that the subsystem lowers to that position
@@ -642,7 +711,7 @@
this->RunFor(chrono::seconds(2));
EXPECT_EQ(this->subsystem()->goal(0), kRange.upper - 0.05);
EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
- EXPECT_NEAR(kRange.upper - 0.05, this->subsystem_status_fetcher_->status.position,
+ EXPECT_NEAR(kRange.upper - 0.05, this->subsystem_status_fetcher_->position(),
0.001);
// Clear max position and check that the subsystem returns to kRange.upper
@@ -650,7 +719,7 @@
this->RunFor(chrono::seconds(2));
EXPECT_EQ(this->subsystem()->goal(0), kRange.upper);
EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
- EXPECT_NEAR(kRange.upper, this->subsystem_status_fetcher_->status.position,
+ EXPECT_NEAR(kRange.upper, this->subsystem_status_fetcher_->position(),
0.001);
}
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.fbs b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.fbs
new file mode 100644
index 0000000..e39272a
--- /dev/null
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.fbs
@@ -0,0 +1,13 @@
+include "frc971/control_loops/profiled_subsystem.fbs";
+include "frc971/control_loops/control_loops.fbs";
+
+namespace frc971.control_loops.zeroing.testing;
+
+table SubsystemGoal {
+ unsafe_goal:double;
+ profile_params:frc971.ProfileParameters;
+}
+
+table SubsystemOutput {
+ output:double;
+}
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.q b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.q
deleted file mode 100644
index 3b35837..0000000
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.q
+++ /dev/null
@@ -1,46 +0,0 @@
-package frc971.control_loops;
-
-import "frc971/control_loops/profiled_subsystem.q";
-
-message StaticZeroingSingleDOFProfiledSubsystemTestGoal {
- double unsafe_goal;
- .frc971.ProfileParameters profile_params;
-};
-
-message StaticZeroingSingleDOFProfiledSubsystemOutput {
- double output;
-};
-
-queue_group StaticZeroingSingleDOFProfiledSubsystemPotAndAbsoluteEncoderTestQueueGroup {
- implements aos.control_loops.ControlLoop;
-
- message Status {
- PotAndAbsoluteEncoderProfiledJointStatus status;
- };
-
- message Position {
- PotAndAbsolutePosition position;
- };
-
- queue StaticZeroingSingleDOFProfiledSubsystemTestGoal goal;
- queue StaticZeroingSingleDOFProfiledSubsystemOutput output;
- queue Status status;
- queue Position position;
-};
-
-queue_group StaticZeroingSingleDOFProfiledSubsystemAbsoluteEncoderTestQueueGroup {
- implements aos.control_loops.ControlLoop;
-
- message Status {
- AbsoluteEncoderProfiledJointStatus status;
- };
-
- message Position {
- AbsolutePosition position;
- };
-
- queue StaticZeroingSingleDOFProfiledSubsystemTestGoal goal;
- queue StaticZeroingSingleDOFProfiledSubsystemOutput output;
- queue Status status;
- queue Position position;
-};
diff --git a/frc971/control_loops/voltage_cap/voltage_cap_test.cc b/frc971/control_loops/voltage_cap/voltage_cap_test.cc
index 9ef4036..73898f9 100644
--- a/frc971/control_loops/voltage_cap/voltage_cap_test.cc
+++ b/frc971/control_loops/voltage_cap/voltage_cap_test.cc
@@ -4,7 +4,6 @@
#include "gtest/gtest.h"
-#include "aos/queue.h"
#include "aos/testing/test_shm.h"
namespace frc971 {