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/autonomous/BUILD b/frc971/autonomous/BUILD
index 2d74bf2..2eb4be0 100644
--- a/frc971/autonomous/BUILD
+++ b/frc971/autonomous/BUILD
@@ -1,15 +1,11 @@
package(default_visibility = ["//visibility:public"])
-load("//aos/build:queues.bzl", "queue_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
-queue_library(
- name = "auto_queue",
- srcs = [
- "auto.q",
- ],
- deps = [
- "//aos/actions:action_queue",
- ],
+flatbuffer_cc_library(
+ name = "auto_fbs",
+ srcs = ["auto.fbs"],
+ visibility = ["//visibility:public"],
)
cc_library(
@@ -21,13 +17,15 @@
"base_autonomous_actor.h",
],
deps = [
- ":auto_queue",
+ ":auto_fbs",
"//aos/actions:action_lib",
"//aos/logging",
"//aos/util:phased_loop",
+ "//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops/drivetrain:drivetrain_config",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
- "//frc971/control_loops/drivetrain:localizer_queue",
- "//y2019/control_loops/drivetrain:target_selector_queue",
+ "//frc971/control_loops/drivetrain:drivetrain_goal_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ "//frc971/control_loops/drivetrain:localizer_fbs",
+ "//y2019/control_loops/drivetrain:target_selector_fbs",
],
)
diff --git a/frc971/autonomous/auto.fbs b/frc971/autonomous/auto.fbs
new file mode 100644
index 0000000..92ca532
--- /dev/null
+++ b/frc971/autonomous/auto.fbs
@@ -0,0 +1,17 @@
+namespace frc971.autonomous;
+
+// Published on ".frc971.autonomous.auto_mode"
+table AutonomousMode {
+ // Mode read from the mode setting sensors.
+ mode:int;
+}
+
+table AutonomousActionParams {
+ // The mode from the sensors when auto starts.
+ mode:int;
+}
+
+table Goal {
+ run:uint;
+ params:AutonomousActionParams;
+}
diff --git a/frc971/autonomous/auto.q b/frc971/autonomous/auto.q
deleted file mode 100644
index f107c90..0000000
--- a/frc971/autonomous/auto.q
+++ /dev/null
@@ -1,26 +0,0 @@
-package frc971.autonomous;
-
-import "aos/actions/actions.q";
-
-// Published on ".frc971.autonomous.auto_mode"
-message AutonomousMode {
- // Mode read from the mode setting sensors.
- int32_t mode;
-};
-
-struct AutonomousActionParams {
- // The mode from the sensors when auto starts.
- int32_t mode;
-};
-
-queue_group AutonomousActionQueueGroup {
- implements aos.common.actions.ActionQueueGroup;
-
- message Goal {
- uint32_t run;
- AutonomousActionParams params;
- };
-
- queue Goal goal;
- queue aos.common.actions.Status status;
-};
diff --git a/frc971/autonomous/base_autonomous_actor.cc b/frc971/autonomous/base_autonomous_actor.cc
index 367e849..dee2931 100644
--- a/frc971/autonomous/base_autonomous_actor.cc
+++ b/frc971/autonomous/base_autonomous_actor.cc
@@ -8,13 +8,15 @@
#include "aos/util/phased_loop.h"
#include "aos/logging/logging.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/control_loops/drivetrain/localizer.q.h"
-#include "y2019/control_loops/drivetrain/target_selector.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "y2019/control_loops/drivetrain/target_selector_generated.h"
using ::aos::monotonic_clock;
namespace chrono = ::std::chrono;
namespace this_thread = ::std::this_thread;
+namespace drivetrain = frc971::control_loops::drivetrain;
namespace frc971 {
namespace autonomous {
@@ -22,41 +24,37 @@
BaseAutonomousActor::BaseAutonomousActor(
::aos::EventLoop *event_loop,
const control_loops::drivetrain::DrivetrainConfig<double> &dt_config)
- : aos::common::actions::ActorBase<AutonomousActionQueueGroup>(
- event_loop, ".frc971.autonomous.autonomous_action"),
+ : aos::common::actions::ActorBase<Goal>(event_loop, "/autonomous"),
dt_config_(dt_config),
initial_drivetrain_({0.0, 0.0}),
target_selector_hint_sender_(
event_loop->MakeSender<
::y2019::control_loops::drivetrain::TargetSelectorHint>(
- ".y2019.control_loops.drivetrain.target_selector_hint")),
+ "/drivetrain")),
drivetrain_goal_sender_(
- event_loop
- ->MakeSender<::frc971::control_loops::DrivetrainQueue::Goal>(
- ".frc971.control_loops.drivetrain_queue.goal")),
+ event_loop->MakeSender<drivetrain::Goal>("/drivetrain")),
drivetrain_status_fetcher_(
- event_loop
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
- ".frc971.control_loops.drivetrain_queue.status")),
+ event_loop->MakeFetcher<drivetrain::Status>("/drivetrain")),
drivetrain_goal_fetcher_(
- event_loop
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Goal>(
- ".frc971.control_loops.drivetrain_queue.goal")) {}
+ event_loop->MakeFetcher<drivetrain::Goal>("/drivetrain")) {}
void BaseAutonomousActor::ResetDrivetrain() {
AOS_LOG(INFO, "resetting the drivetrain\n");
max_drivetrain_voltage_ = 12.0;
goal_spline_handle_ = 0;
- auto drivetrain_goal_message = drivetrain_goal_sender_.MakeMessage();
- drivetrain_goal_message->controller_type = 0;
- drivetrain_goal_message->highgear = true;
- drivetrain_goal_message->wheel = 0.0;
- drivetrain_goal_message->throttle = 0.0;
- drivetrain_goal_message->left_goal = initial_drivetrain_.left;
- drivetrain_goal_message->right_goal = initial_drivetrain_.right;
- drivetrain_goal_message->max_ss_voltage = max_drivetrain_voltage_;
- drivetrain_goal_message.Send();
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ drivetrain::Goal::Builder goal_builder =
+ builder.MakeBuilder<drivetrain::Goal>();
+ goal_builder.add_controller_type(drivetrain::ControllerType_POLYDRIVE);
+ goal_builder.add_highgear(true);
+ goal_builder.add_wheel(0.0);
+ goal_builder.add_throttle(0.0);
+ goal_builder.add_left_goal(initial_drivetrain_.left);
+ goal_builder.add_right_goal(initial_drivetrain_.right);
+ goal_builder.add_max_ss_voltage(max_drivetrain_voltage_);
+ builder.Send(goal_builder.Finish());
}
void BaseAutonomousActor::InitializeEncoders() {
@@ -64,14 +62,14 @@
WaitUntil([this]() { return drivetrain_status_fetcher_.Fetch(); });
initial_drivetrain_.left =
- drivetrain_status_fetcher_->estimated_left_position;
+ drivetrain_status_fetcher_->estimated_left_position();
initial_drivetrain_.right =
- drivetrain_status_fetcher_->estimated_right_position;
+ drivetrain_status_fetcher_->estimated_right_position();
}
void BaseAutonomousActor::StartDrive(double distance, double angle,
- ProfileParameters linear,
- ProfileParameters angular) {
+ ProfileParametersT linear,
+ ProfileParametersT angular) {
AOS_LOG(INFO, "Driving distance %f, angle %f\n", distance, angle);
{
const double dangle = angle * dt_config_.robot_radius;
@@ -79,20 +77,25 @@
initial_drivetrain_.right += distance + dangle;
}
- auto drivetrain_message = drivetrain_goal_sender_.MakeMessage();
- drivetrain_message->controller_type = 1;
- drivetrain_message->highgear = true;
- drivetrain_message->wheel = 0.0;
- drivetrain_message->throttle = 0.0;
- drivetrain_message->left_goal = initial_drivetrain_.left;
- drivetrain_message->right_goal = initial_drivetrain_.right;
- drivetrain_message->max_ss_voltage = max_drivetrain_voltage_;
- drivetrain_message->linear = linear;
- drivetrain_message->angular = angular;
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
- AOS_LOG_STRUCT(DEBUG, "dtg", *drivetrain_message);
+ auto linear_offset = ProfileParameters::Pack(*builder.fbb(), &linear);
+ auto angular_offset = ProfileParameters::Pack(*builder.fbb(), &angular);
- drivetrain_message.Send();
+ drivetrain::Goal::Builder goal_builder =
+ builder.MakeBuilder<drivetrain::Goal>();
+
+ goal_builder.add_controller_type(drivetrain::ControllerType_MOTION_PROFILE);
+ goal_builder.add_highgear(true);
+ goal_builder.add_wheel(0.0);
+ goal_builder.add_throttle(0.0);
+ goal_builder.add_left_goal(initial_drivetrain_.left);
+ goal_builder.add_right_goal(initial_drivetrain_.right);
+ goal_builder.add_max_ss_voltage(max_drivetrain_voltage_);
+ goal_builder.add_linear(linear_offset);
+ goal_builder.add_angular(angular_offset);
+
+ builder.Send(goal_builder.Finish());
}
void BaseAutonomousActor::WaitUntilDoneOrCanceled(
@@ -137,17 +140,17 @@
static constexpr double kProfileTolerance = 0.001;
if (drivetrain_status_fetcher_.get()) {
- if (::std::abs(drivetrain_status_fetcher_->profiled_left_position_goal -
+ if (::std::abs(drivetrain_status_fetcher_->profiled_left_position_goal() -
initial_drivetrain_.left) < kProfileTolerance &&
- ::std::abs(drivetrain_status_fetcher_->profiled_right_position_goal -
+ ::std::abs(drivetrain_status_fetcher_->profiled_right_position_goal() -
initial_drivetrain_.right) < kProfileTolerance &&
- ::std::abs(drivetrain_status_fetcher_->estimated_left_position -
+ ::std::abs(drivetrain_status_fetcher_->estimated_left_position() -
initial_drivetrain_.left) < kPositionTolerance &&
- ::std::abs(drivetrain_status_fetcher_->estimated_right_position -
+ ::std::abs(drivetrain_status_fetcher_->estimated_right_position() -
initial_drivetrain_.right) < kPositionTolerance &&
- ::std::abs(drivetrain_status_fetcher_->estimated_left_velocity) <
+ ::std::abs(drivetrain_status_fetcher_->estimated_left_velocity()) <
kVelocityTolerance &&
- ::std::abs(drivetrain_status_fetcher_->estimated_right_velocity) <
+ ::std::abs(drivetrain_status_fetcher_->estimated_right_velocity()) <
kVelocityTolerance) {
AOS_LOG(INFO, "Finished drive\n");
return true;
@@ -170,7 +173,7 @@
return true;
}
if (drivetrain_status_fetcher_.get()) {
- if (drivetrain_status_fetcher_->ground_angle > angle) {
+ if (drivetrain_status_fetcher_->ground_angle() > angle) {
return true;
}
}
@@ -191,7 +194,7 @@
return true;
}
if (drivetrain_status_fetcher_.get()) {
- if (drivetrain_status_fetcher_->ground_angle < angle) {
+ if (drivetrain_status_fetcher_->ground_angle() < angle) {
return true;
}
}
@@ -213,10 +216,10 @@
return true;
}
if (drivetrain_status_fetcher_.get()) {
- if (drivetrain_status_fetcher_->ground_angle > max_angle) {
- max_angle = drivetrain_status_fetcher_->ground_angle;
+ if (drivetrain_status_fetcher_->ground_angle() > max_angle) {
+ max_angle = drivetrain_status_fetcher_->ground_angle();
}
- if (drivetrain_status_fetcher_->ground_angle < max_angle - angle) {
+ if (drivetrain_status_fetcher_->ground_angle() < max_angle - angle) {
return true;
}
}
@@ -243,17 +246,17 @@
if (drivetrain_status_fetcher_.get()) {
const double left_profile_error =
(initial_drivetrain_.left -
- drivetrain_status_fetcher_->profiled_left_position_goal);
+ drivetrain_status_fetcher_->profiled_left_position_goal());
const double right_profile_error =
(initial_drivetrain_.right -
- drivetrain_status_fetcher_->profiled_right_position_goal);
+ drivetrain_status_fetcher_->profiled_right_position_goal());
const double left_error =
(initial_drivetrain_.left -
- drivetrain_status_fetcher_->estimated_left_position);
+ drivetrain_status_fetcher_->estimated_left_position());
const double right_error =
(initial_drivetrain_.right -
- drivetrain_status_fetcher_->estimated_right_position);
+ drivetrain_status_fetcher_->estimated_right_position());
const double profile_distance_to_go =
(left_profile_error + right_profile_error) / 2.0;
@@ -306,9 +309,9 @@
const Eigen::Matrix<double, 7, 1> current_error =
(Eigen::Matrix<double, 7, 1>()
<< initial_drivetrain_.left -
- drivetrain_status_fetcher_->profiled_left_position_goal,
+ drivetrain_status_fetcher_->profiled_left_position_goal(),
0.0, initial_drivetrain_.right -
- drivetrain_status_fetcher_->profiled_right_position_goal,
+ drivetrain_status_fetcher_->profiled_right_position_goal(),
0.0, 0.0, 0.0, 0.0)
.finished();
const Eigen::Matrix<double, 2, 1> linear_error =
@@ -342,9 +345,9 @@
const Eigen::Matrix<double, 7, 1> current_error =
(Eigen::Matrix<double, 7, 1>()
<< initial_drivetrain_.left -
- drivetrain_status_fetcher_->profiled_left_position_goal,
+ drivetrain_status_fetcher_->profiled_left_position_goal(),
0.0, initial_drivetrain_.right -
- drivetrain_status_fetcher_->profiled_right_position_goal,
+ drivetrain_status_fetcher_->profiled_right_position_goal(),
0.0, 0.0, 0.0, 0.0)
.finished();
const Eigen::Matrix<double, 2, 1> angular_error =
@@ -369,10 +372,10 @@
if (drivetrain_status_fetcher_.get()) {
const double left_error =
(initial_drivetrain_.left -
- drivetrain_status_fetcher_->estimated_left_position);
+ drivetrain_status_fetcher_->estimated_left_position());
const double right_error =
(initial_drivetrain_.right -
- drivetrain_status_fetcher_->estimated_right_position);
+ drivetrain_status_fetcher_->estimated_right_position());
return (left_error + right_error) / 2.0;
} else {
@@ -385,9 +388,11 @@
base_autonomous_actor_->drivetrain_status_fetcher_.Fetch();
if (base_autonomous_actor_->drivetrain_status_fetcher_.get()) {
return base_autonomous_actor_->drivetrain_status_fetcher_
- ->trajectory_logging.is_executing &&
+ ->trajectory_logging()
+ ->is_executing() &&
base_autonomous_actor_->drivetrain_status_fetcher_
- ->trajectory_logging.distance_remaining < distance;
+ ->trajectory_logging()
+ ->distance_remaining() < distance;
}
return false;
}
@@ -408,61 +413,89 @@
}
}
-void BaseAutonomousActor::LineFollowAtVelocity(double velocity, int hint) {
- auto drivetrain_message = drivetrain_goal_sender_.MakeMessage();
- drivetrain_message->controller_type = 3;
- // TODO(james): Currently the 4.0 is copied from the
- // line_follow_drivetrain.cc, but it is somewhat year-specific, so we should
- // factor it out in some way.
- drivetrain_message->throttle = velocity / 4.0;
- drivetrain_message.Send();
- auto target_hint = target_selector_hint_sender_.MakeMessage();
- target_hint->suggested_target = hint;
- target_hint.Send();
+void BaseAutonomousActor::LineFollowAtVelocity(
+ double velocity, y2019::control_loops::drivetrain::SelectionHint hint) {
+ {
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+ drivetrain::Goal::Builder goal_builder =
+ builder.MakeBuilder<drivetrain::Goal>();
+ goal_builder.add_controller_type(
+ drivetrain::ControllerType_SPLINE_FOLLOWER);
+ // TODO(james): Currently the 4.0 is copied from the
+ // line_follow_drivetrain.cc, but it is somewhat year-specific, so we should
+ // factor it out in some way.
+ goal_builder.add_throttle(velocity / 4.0);
+ builder.Send(goal_builder.Finish());
+ }
+
+ {
+ auto builder = target_selector_hint_sender_.MakeBuilder();
+ ::y2019::control_loops::drivetrain::TargetSelectorHint::Builder
+ target_hint_builder = builder.MakeBuilder<
+ ::y2019::control_loops::drivetrain::TargetSelectorHint>();
+
+ target_hint_builder.add_suggested_target(hint);
+ builder.Send(target_hint_builder.Finish());
+ }
}
BaseAutonomousActor::SplineHandle BaseAutonomousActor::PlanSpline(
- const ::frc971::MultiSpline &spline, SplineDirection direction) {
+ std::function<flatbuffers::Offset<frc971::MultiSpline>(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder)>
+ &&multispline_builder,
+ SplineDirection direction) {
AOS_LOG(INFO, "Planning spline\n");
int32_t spline_handle = (++spline_handle_) | ((getpid() & 0xFFFF) << 15);
drivetrain_goal_fetcher_.Fetch();
- auto drivetrain_message =
- drivetrain_goal_sender_.MakeMessage();
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
- int controller_type = 2;
+ flatbuffers::Offset<frc971::MultiSpline> multispline_offset =
+ multispline_builder(&builder);
+
+ drivetrain::SplineGoal::Builder spline_builder =
+ builder.MakeBuilder<drivetrain::SplineGoal>();
+ spline_builder.add_spline_idx(spline_handle);
+ spline_builder.add_drive_spline_backwards(direction ==
+ SplineDirection::kBackward);
+ spline_builder.add_spline(multispline_offset);
+
+ flatbuffers::Offset<drivetrain::SplineGoal> spline_offset =
+ spline_builder.Finish();
+
+ drivetrain::Goal::Builder goal_builder =
+ builder.MakeBuilder<drivetrain::Goal>();
+
+ drivetrain::ControllerType controller_type =
+ drivetrain::ControllerType_SPLINE_FOLLOWER;
if (drivetrain_goal_fetcher_.get()) {
- controller_type = drivetrain_goal_fetcher_->controller_type;
- drivetrain_message->throttle = drivetrain_goal_fetcher_->throttle;
+ controller_type = drivetrain_goal_fetcher_->controller_type();
+ goal_builder.add_throttle(drivetrain_goal_fetcher_->throttle());
}
- drivetrain_message->controller_type = controller_type;
+ goal_builder.add_controller_type(controller_type);
+ goal_builder.add_spline_handle(goal_spline_handle_);
- drivetrain_message->spline = spline;
- drivetrain_message->spline.spline_idx = spline_handle;
- drivetrain_message->spline_handle = goal_spline_handle_;
- drivetrain_message->spline.drive_spline_backwards =
- direction == SplineDirection::kBackward;
+ goal_builder.add_spline(spline_offset);
- AOS_LOG_STRUCT(DEBUG, "dtg", *drivetrain_message);
-
- drivetrain_message.Send();
+ builder.Send(goal_builder.Finish());
return BaseAutonomousActor::SplineHandle(spline_handle, this);
}
bool BaseAutonomousActor::SplineHandle::IsPlanned() {
base_autonomous_actor_->drivetrain_status_fetcher_.Fetch();
- AOS_LOG_STRUCT(DEBUG, "dts",
- *(base_autonomous_actor_->drivetrain_status_fetcher_.get()));
+ VLOG(1) << aos::FlatbufferToJson(
+ base_autonomous_actor_->drivetrain_status_fetcher_.get());
+
if (base_autonomous_actor_->drivetrain_status_fetcher_.get() &&
- ((base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging
- .planning_spline_idx == spline_handle_ &&
- base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging
- .planning_state == 3) ||
- base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging
- .current_spline_idx == spline_handle_)) {
+ ((base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging()
+ ->planning_spline_idx() == spline_handle_ &&
+ base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging()
+ ->planning_state() == 3) ||
+ base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging()
+ ->current_spline_idx() == spline_handle_)) {
return true;
}
return false;
@@ -485,24 +518,21 @@
}
void BaseAutonomousActor::SplineHandle::Start() {
- auto drivetrain_message =
- base_autonomous_actor_->drivetrain_goal_sender_.MakeMessage();
- drivetrain_message->controller_type = 2;
+ auto builder = base_autonomous_actor_->drivetrain_goal_sender_.MakeBuilder();
+ drivetrain::Goal::Builder goal_builder =
+ builder.MakeBuilder<drivetrain::Goal>();
+ goal_builder.add_controller_type(drivetrain::ControllerType_SPLINE_FOLLOWER);
AOS_LOG(INFO, "Starting spline\n");
- drivetrain_message->spline_handle = spline_handle_;
+ goal_builder.add_spline_handle(spline_handle_);
base_autonomous_actor_->goal_spline_handle_ = spline_handle_;
- AOS_LOG_STRUCT(DEBUG, "dtg", *drivetrain_message);
-
- drivetrain_message.Send();
+ builder.Send(goal_builder.Finish());
}
bool BaseAutonomousActor::SplineHandle::IsDone() {
base_autonomous_actor_->drivetrain_status_fetcher_.Fetch();
- AOS_LOG_STRUCT(INFO, "dts",
- *(base_autonomous_actor_->drivetrain_status_fetcher_.get()));
// We check that the spline we are waiting on is neither currently planning
// nor executing (we check is_executed because it is possible to receive
@@ -510,12 +540,13 @@
// We check for planning so that the user can go straight from starting the
// planner to executing without a WaitForPlan in between.
if (base_autonomous_actor_->drivetrain_status_fetcher_.get() &&
- ((!base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging
- .is_executed &&
- base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging
- .current_spline_idx == spline_handle_) ||
- base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging
- .planning_spline_idx == spline_handle_)) {
+ ((!base_autonomous_actor_->drivetrain_status_fetcher_
+ ->trajectory_logging()
+ ->is_executed() &&
+ base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging()
+ ->current_spline_idx() == spline_handle_) ||
+ base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging()
+ ->planning_spline_idx() == spline_handle_)) {
return false;
}
return true;
diff --git a/frc971/autonomous/base_autonomous_actor.h b/frc971/autonomous/base_autonomous_actor.h
index 5a5bcea..6035551 100644
--- a/frc971/autonomous/base_autonomous_actor.h
+++ b/frc971/autonomous/base_autonomous_actor.h
@@ -5,20 +5,20 @@
#include "aos/actions/actions.h"
#include "aos/actions/actor.h"
-#include "aos/events/shm-event-loop.h"
-#include "frc971/autonomous/auto.q.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "aos/events/shm_event_loop.h"
+#include "frc971/autonomous/auto_generated.h"
+#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
-#include "y2019/control_loops/drivetrain/target_selector.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "y2019/control_loops/drivetrain/target_selector_generated.h"
namespace frc971 {
namespace autonomous {
-class BaseAutonomousActor
- : public ::aos::common::actions::ActorBase<AutonomousActionQueueGroup> {
+class BaseAutonomousActor : public ::aos::common::actions::ActorBase<Goal> {
public:
- typedef ::aos::common::actions::TypedActionFactory<AutonomousActionQueueGroup>
- Factory;
+ typedef ::aos::common::actions::TypedActionFactory<Goal> Factory;
explicit BaseAutonomousActor(
::aos::EventLoop *event_loop,
@@ -62,13 +62,16 @@
// Starts planning the spline, and returns a handle to be used to manipulate
// it.
- SplineHandle PlanSpline(const ::frc971::MultiSpline &spline,
- SplineDirection direction);
+ SplineHandle PlanSpline(
+ std::function<flatbuffers::Offset<frc971::MultiSpline>(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder
+ *builder)> &&multispline_builder,
+ SplineDirection direction);
void ResetDrivetrain();
void InitializeEncoders();
- void StartDrive(double distance, double angle, ProfileParameters linear,
- ProfileParameters angular);
+ void StartDrive(double distance, double angle, ProfileParametersT linear,
+ ProfileParametersT angular);
void WaitUntilDoneOrCanceled(
::std::unique_ptr<aos::common::actions::Action> action);
@@ -79,7 +82,10 @@
// Returns true if the drive has finished.
bool IsDriveDone();
- void LineFollowAtVelocity(double velocity, int hint = 0);
+ void LineFollowAtVelocity(
+ double velocity,
+ y2019::control_loops::drivetrain::SelectionHint hint =
+ y2019::control_loops::drivetrain::SelectionHint_NONE);
// Waits until the robot is pitched up above the specified angle, or the move
// finishes. Returns true on success, and false if it cancels.
@@ -115,11 +121,11 @@
::aos::Sender<::y2019::control_loops::drivetrain::TargetSelectorHint>
target_selector_hint_sender_;
- ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Goal>
+ ::aos::Sender<::frc971::control_loops::drivetrain::Goal>
drivetrain_goal_sender_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Goal>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Goal>
drivetrain_goal_fetcher_;
private:
diff --git a/frc971/codelab/BUILD b/frc971/codelab/BUILD
index 0fe9205..6302443 100644
--- a/frc971/codelab/BUILD
+++ b/frc971/codelab/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")
cc_binary(
name = "basic_test",
@@ -8,10 +8,9 @@
srcs = ["basic_test.cc"],
deps = [
":basic",
- ":basic_queue",
- "//aos:queues",
+ ":basic_fbs",
"//aos/controls:control_loop_test",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
"//aos/testing:googletest",
"//frc971/control_loops:state_feedback_loop",
"//frc971/control_loops:team_number_test_environment",
@@ -23,18 +22,14 @@
srcs = ["basic.cc"],
hdrs = ["basic.h"],
deps = [
- ":basic_queue",
+ ":basic_fbs",
"//aos/controls:control_loop",
],
)
-queue_library(
- name = "basic_queue",
+flatbuffer_cc_library(
+ name = "basic_fbs",
srcs = [
- "basic.q",
- ],
- deps = [
- "//aos/controls:control_loop_queues",
- "//frc971/control_loops:queues",
+ "basic.fbs",
],
)
diff --git a/frc971/codelab/basic.cc b/frc971/codelab/basic.cc
index d06e285..29ffcf5 100644
--- a/frc971/codelab/basic.cc
+++ b/frc971/codelab/basic.cc
@@ -4,12 +4,12 @@
namespace codelab {
Basic::Basic(::aos::EventLoop *event_loop, const ::std::string &name)
- : aos::controls::ControlLoop<BasicQueue>(event_loop, name) {}
+ : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+ name) {}
-void Basic::RunIteration(const BasicQueue::Goal *goal,
- const BasicQueue::Position *position,
- BasicQueue::Output *output,
- BasicQueue::Status *status) {
+void Basic::RunIteration(const Goal *goal, const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) {
// TODO(you): Set the intake_voltage to 12 Volts when
// intake is requested (via intake in goal). Make sure not to set
// the motor to anything but 0 V when the limit_sensor is pressed.
diff --git a/frc971/codelab/basic.fbs b/frc971/codelab/basic.fbs
new file mode 100644
index 0000000..82c9607
--- /dev/null
+++ b/frc971/codelab/basic.fbs
@@ -0,0 +1,35 @@
+namespace frc971.codelab;
+
+// The theme of this basic test is a simple intake system.
+//
+// The system will have a motor driven by the voltage returned
+// by output, and then eventually this motor, when run enough,
+// will trigger the limit_sensor. The hypothetical motor should shut
+// off in that hypothetical situation to avoid hypothetical burnout.
+table Goal {
+ // The control loop needs to intake now.
+ intake:bool;
+}
+
+table Position {
+ // This is a potential incoming sensor value letting us know
+ // if we need to be intaking.
+ limit_sensor:bool;
+}
+
+table Status {
+ // Lets consumers of basic_queue.status know if
+ // the requested intake is finished.
+ intake_complete:bool;
+}
+
+table Output {
+ // This would be set up to drive a hypothetical motor that would
+ // hope to intake something.
+ intake_voltage:double;
+}
+
+root_type Goal;
+root_type Position;
+root_type Status;
+root_type Output;
diff --git a/frc971/codelab/basic.h b/frc971/codelab/basic.h
index 6f33718..acaec0c 100644
--- a/frc971/codelab/basic.h
+++ b/frc971/codelab/basic.h
@@ -4,7 +4,7 @@
#include "aos/controls/control_loop.h"
#include "aos/time/time.h"
-#include "frc971/codelab/basic.q.h"
+#include "frc971/codelab/basic_generated.h"
namespace frc971 {
namespace codelab {
@@ -41,16 +41,16 @@
// Order of approaching this should be:
// - Read the BUILD file and learn about what code is being generated.
// - Read basic.q, and familiarize yourself on the inputs and types involved.
-class Basic : public ::aos::controls::ControlLoop<BasicQueue> {
+class Basic
+ : public ::aos::controls::ControlLoop<Goal, Position, Status, Output> {
public:
explicit Basic(::aos::EventLoop *event_loop,
- const ::std::string &name = ".frc971.codelab.basic_queue");
+ const ::std::string &name = "/codelab");
protected:
- void RunIteration(const BasicQueue::Goal *goal,
- const BasicQueue::Position *position,
- BasicQueue::Output *output,
- BasicQueue::Status *status) override;
+ void RunIteration(const Goal *goal, const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) override;
};
} // namespace codelab
diff --git a/frc971/codelab/basic.q b/frc971/codelab/basic.q
deleted file mode 100644
index 58fd69e..0000000
--- a/frc971/codelab/basic.q
+++ /dev/null
@@ -1,44 +0,0 @@
-package frc971.codelab;
-
-import "aos/controls/control_loops.q";
-import "frc971/control_loops/control_loops.q";
-
-// The theme of this basic test is a simple intake system.
-//
-// The system will have a motor driven by the voltage returned
-// by output, and then eventually this motor, when run enough,
-// will trigger the limit_sensor. The hypothetical motor should shut
-// off in that hypothetical situation to avoid hypothetical burnout.
-queue_group BasicQueue {
- implements aos.control_loops.ControlLoop;
-
- message Goal {
- // The control loop needs to intake now.
- bool intake;
- };
-
- message Position {
- // This is a potential incoming sensor value letting us know
- // if we need to be intaking.
- bool limit_sensor;
- };
-
- message Status {
- // Lets consumers of basic_queue.status know if
- // the requested intake is finished.
- bool intake_complete;
- };
-
- message Output {
- // This would be set up to drive a hypothetical motor that would
- // hope to intake something.
- double intake_voltage;
- };
-
- queue Goal goal;
- queue Position position;
- queue Output output;
- queue Status status;
-};
-
-queue_group BasicQueue basic_queue;
diff --git a/frc971/codelab/basic_test.cc b/frc971/codelab/basic_test.cc
index 91059f6..487a09b 100644
--- a/frc971/codelab/basic_test.cc
+++ b/frc971/codelab/basic_test.cc
@@ -6,9 +6,8 @@
#include <memory>
#include "aos/controls/control_loop_test.h"
-#include "aos/events/shm-event-loop.h"
-#include "aos/queue.h"
-#include "frc971/codelab/basic.q.h"
+#include "aos/events/shm_event_loop.h"
+#include "frc971/codelab/basic_generated.h"
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
@@ -25,12 +24,9 @@
public:
BasicSimulation(::aos::EventLoop *event_loop, chrono::nanoseconds dt)
: event_loop_(event_loop),
- position_sender_(event_loop_->MakeSender<BasicQueue::Position>(
- ".frc971.codelab.basic_queue.position")),
- status_fetcher_(event_loop_->MakeFetcher<BasicQueue::Status>(
- ".frc971.codelab.basic_queue.status")),
- output_fetcher_(event_loop_->MakeFetcher<BasicQueue::Output>(
- ".frc971.codelab.basic_queue.output")) {
+ position_sender_(event_loop_->MakeSender<Position>("/codelab")),
+ status_fetcher_(event_loop_->MakeFetcher<Status>("/codelab")),
+ output_fetcher_(event_loop_->MakeFetcher<Output>("/codelab")) {
event_loop_->AddPhasedLoop(
[this](int) {
// Skip this the first time.
@@ -45,11 +41,13 @@
// Sends a queue message with the position data.
void SendPositionMessage() {
- auto position = position_sender_.MakeMessage();
+ auto builder = position_sender_.MakeBuilder();
- position->limit_sensor = limit_sensor_;
+ Position::Builder position_builder = builder.MakeBuilder<Position>();
- position.Send();
+ position_builder.add_limit_sensor(limit_sensor_);
+
+ builder.Send(position_builder.Finish());
}
void VerifyResults(double voltage, bool status) {
@@ -59,8 +57,8 @@
ASSERT_TRUE(output_fetcher_.get() != nullptr);
ASSERT_TRUE(status_fetcher_.get() != nullptr);
- EXPECT_EQ(output_fetcher_->intake_voltage, voltage);
- EXPECT_EQ(status_fetcher_->intake_complete, status);
+ EXPECT_EQ(output_fetcher_->intake_voltage(), voltage);
+ EXPECT_EQ(status_fetcher_->intake_complete(), status);
}
void set_limit_sensor(bool value) { limit_sensor_ = value; }
@@ -71,9 +69,9 @@
private:
::aos::EventLoop *event_loop_;
- ::aos::Sender<BasicQueue::Position> position_sender_;
- ::aos::Fetcher<BasicQueue::Status> status_fetcher_;
- ::aos::Fetcher<BasicQueue::Output> output_fetcher_;
+ ::aos::Sender<Position> position_sender_;
+ ::aos::Fetcher<Status> status_fetcher_;
+ ::aos::Fetcher<Output> output_fetcher_;
bool limit_sensor_ = false;
@@ -83,13 +81,41 @@
class BasicControlLoopTest : public ::aos::testing::ControlLoopTest {
public:
BasicControlLoopTest()
- : ::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\": \"/codelab\",\n"
+ " \"type\": \"frc971.codelab.Goal\"\n"
+ " },\n"
+ " {\n"
+ " \"name\": \"/codelab\",\n"
+ " \"type\": \"frc971.codelab.Output\"\n"
+ " },\n"
+ " {\n"
+ " \"name\": \"/codelab\",\n"
+ " \"type\": \"frc971.codelab.Status\"\n"
+ " },\n"
+ " {\n"
+ " \"name\": \"/codelab\",\n"
+ " \"type\": \"frc971.codelab.Position\"\n"
+ " }\n"
+ " ]\n"
+ "}\n",
+ chrono::microseconds(5050)),
test_event_loop_(MakeEventLoop()),
- goal_sender_(test_event_loop_->MakeSender<BasicQueue::Goal>(
- ".frc971.codelab.basic_queue.goal")),
+ goal_sender_(test_event_loop_->MakeSender<Goal>("/codelab")),
basic_event_loop_(MakeEventLoop()),
- basic_(basic_event_loop_.get(), ".frc971.codelab.basic_queue"),
+ basic_(basic_event_loop_.get(), "/codelab"),
basic_simulation_event_loop_(MakeEventLoop()),
basic_simulation_(basic_simulation_event_loop_.get(), dt()) {
@@ -97,7 +123,7 @@
}
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
- ::aos::Sender<BasicQueue::Goal> goal_sender_;
+ ::aos::Sender<Goal> goal_sender_;
::std::unique_ptr<::aos::EventLoop> basic_event_loop_;
Basic basic_;
@@ -109,9 +135,10 @@
// Tests that when the motor has finished intaking it stops.
TEST_F(BasicControlLoopTest, IntakeLimitTransitionsToTrue) {
{
- auto message = goal_sender_.MakeMessage();
- message->intake = true;
- ASSERT_TRUE(message.Send());
+ auto builder = goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(true);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
basic_simulation_.set_limit_sensor(false);
@@ -121,9 +148,10 @@
basic_simulation_.VerifyResults(12.0, false);
{
- auto message = goal_sender_.MakeMessage();
- message->intake = true;
- ASSERT_TRUE(message.Send());
+ auto builder = goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(true);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
basic_simulation_.set_limit_sensor(true);
@@ -136,9 +164,10 @@
// and intake is requested.
TEST_F(BasicControlLoopTest, IntakeLimitNotSet) {
{
- auto message = goal_sender_.MakeMessage();
- message->intake = true;
- ASSERT_TRUE(message.Send());
+ auto builder = goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(true);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
basic_simulation_.set_limit_sensor(false);
@@ -151,9 +180,10 @@
// even if the limit sensor is off.
TEST_F(BasicControlLoopTest, NoIntakeLimitNotSet) {
{
- auto message = goal_sender_.MakeMessage();
- message->intake = false;
- ASSERT_TRUE(message.Send());
+ auto builder = goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(false);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
basic_simulation_.set_limit_sensor(false);
@@ -166,9 +196,10 @@
// is pressed and intake is requested.
TEST_F(BasicControlLoopTest, IntakeLimitSet) {
{
- auto message = goal_sender_.MakeMessage();
- message->intake = true;
- ASSERT_TRUE(message.Send());
+ auto builder = goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(true);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
basic_simulation_.set_limit_sensor(true);
@@ -180,9 +211,10 @@
// Tests that the intake is off if no intake is requested,
TEST_F(BasicControlLoopTest, NoIntakeLimitSet) {
{
- auto message = goal_sender_.MakeMessage();
- message->intake = false;
- ASSERT_TRUE(message.Send());
+ auto builder = goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(false);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
basic_simulation_.set_limit_sensor(true);
@@ -205,9 +237,10 @@
basic_simulation_.set_limit_sensor(true);
{
- auto message = goal_sender_.MakeMessage();
- message->intake = true;
- ASSERT_TRUE(message.Send());
+ auto builder = goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(true);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
SetEnabled(false);
diff --git a/frc971/config/10-net-eth0.rules b/frc971/config/10-net-eth0.rules
new file mode 100644
index 0000000..0d8d807
--- /dev/null
+++ b/frc971/config/10-net-eth0.rules
@@ -0,0 +1,6 @@
+# This is a file that will make any NIC eth0.
+# It prevents the persistent net rules generator from running because that ends
+# up naming the 1 NIC eth1 instead of when you move a disk between boxes.
+# Put it in /etc/udev/rules.d/ to use it.
+
+SUBSYSTEM=="net", ACTION=="add", ATTR{type}=="1", KERNEL=="eth*", NAME="eth0"
diff --git a/frc971/config/BUILD b/frc971/config/BUILD
new file mode 100644
index 0000000..07394d4
--- /dev/null
+++ b/frc971/config/BUILD
@@ -0,0 +1,14 @@
+filegroup(
+ name = 'rio_robotCommand',
+ srcs = [ 'robotCommand' ],
+)
+
+sh_binary(
+ name = 'setup_roborio',
+ srcs = [ 'setup_roborio.sh' ],
+ visibility = [ '//visibility:public' ],
+ data = [
+ ':rio_robotCommand',
+ '@arm_frc_linux_gnueabi_repo//:compiler_pieces',
+ ],
+)
diff --git a/frc971/config/aos.conf b/frc971/config/aos.conf
new file mode 100644
index 0000000..bda250f
--- /dev/null
+++ b/frc971/config/aos.conf
@@ -0,0 +1,6 @@
+# put this file in /etc/security/limits.d/ to make it work
+# you have to create a group named aos (groupadd aos) and then add anybody you want to it (vim /etc/group)
+# See limits.conf(5) for details.
+
+@aos hard memlock unlimited
+@aos hard rtprio 40
diff --git a/frc971/config/robotCommand b/frc971/config/robotCommand
new file mode 100755
index 0000000..7b726a7
--- /dev/null
+++ b/frc971/config/robotCommand
@@ -0,0 +1 @@
+/home/admin/robot_code/starter.sh
diff --git a/frc971/config/setup_roborio.sh b/frc971/config/setup_roborio.sh
new file mode 100755
index 0000000..70c4a73
--- /dev/null
+++ b/frc971/config/setup_roborio.sh
@@ -0,0 +1,41 @@
+#!/bin/bash
+#
+# Note: this should be run from within bazel
+
+set -Eeuo pipefail
+
+if [ $# != 1 ];
+then
+ echo "# setup_robot.sh is used to configure a newly flashed roboRIO"
+ echo ""
+ echo "Usage: setup_roborio.sh 10.9.71.2"
+ echo ""
+ echo "# or if that does not work, try"
+ echo ""
+ echo "Usage: setup_roborio.sh roboRIO-971-frc.local"
+ exit 1
+fi
+
+readonly ROBOT_HOSTNAME="$1"
+
+echo "Looking to see if l is aliased right."
+
+if ! HAS_ALIAS=$(ssh "admin@${ROBOT_HOSTNAME}" "cat /etc/profile"); then
+ echo "ssh command failed remotely"
+ exit 1
+elif echo "${HAS_ALIAS}" | grep -Fq "alias l"; then
+ echo "Already has l alias"
+else
+ echo "Adding l alias"
+ ssh "admin@${ROBOT_HOSTNAME}" 'echo "alias l=\"ls -la\"" >> /etc/profile'
+ echo "Adding symbolic link to loging directory"
+ ssh "admin@${ROBOT_HOSTNAME}" ln -s /media/sda1 logs
+ ssh "admin@${ROBOT_HOSTNAME}" mkdir robot_code
+ ssh "admin@${ROBOT_HOSTNAME}" ln -s /media/sda1/aos_log-current robot_code/aos_log-current
+fi
+
+# This fails if the code isn't running.
+ssh "admin@${ROBOT_HOSTNAME}" 'PATH="${PATH}":/usr/local/natinst/bin/ /usr/local/frc/bin/frcKillRobot.sh -r -t' || true
+
+echo "Deploying robotCommand startup script"
+scp aos/config/robotCommand "admin@${ROBOT_HOSTNAME}:/home/lvuser/"
diff --git a/frc971/config/setup_rt_caps.sh b/frc971/config/setup_rt_caps.sh
new file mode 100755
index 0000000..1ba4f65
--- /dev/null
+++ b/frc971/config/setup_rt_caps.sh
@@ -0,0 +1,8 @@
+#!/bin/bash
+
+# Sets capabilities on a binary so it can run as realtime code as a user who
+# doesn't have those rlimits or some other reason they can.
+# Has to be run as root.
+
+setcap 'CAP_IPC_LOCK+pie CAP_SYS_NICE+pie' $0
+
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 {
diff --git a/frc971/downloader.bzl b/frc971/downloader.bzl
index 2967127..842398f58 100644
--- a/frc971/downloader.bzl
+++ b/frc971/downloader.bzl
@@ -1,37 +1,37 @@
-load('//aos/downloader:downloader.bzl', 'aos_downloader')
-load('//tools/build_rules:label.bzl', 'expand_label')
+load("//frc971/downloader:downloader.bzl", "aos_downloader")
+load("//tools/build_rules:label.bzl", "expand_label")
-def robot_downloader(start_binaries, binaries=[], dirs=None, default_target=None):
- '''Sets up the standard robot download targets.
+def robot_downloader(start_binaries, binaries = [], dirs = None, default_target = None):
+ """Sets up the standard robot download targets.
- Attrs:
- start_binaries: A list of cc_binary targets to start on the robot.
- dirs: Passed through to aos_downloader.
- default_target: Passed through to aos_downloader.
- '''
+ Attrs:
+ start_binaries: A list of cc_binary targets to start on the robot.
+ dirs: Passed through to aos_downloader.
+ default_target: Passed through to aos_downloader.
+ """
- aos_downloader(
- name = 'download',
- start_srcs = [
- '//aos:prime_start_binaries',
- ] + start_binaries,
- srcs = [
- '//aos:prime_binaries',
- ] + binaries,
- dirs = dirs,
- default_target = default_target,
- restricted_to = ['//tools:roborio'],
- )
+ aos_downloader(
+ name = "download",
+ start_srcs = [
+ "//aos:prime_start_binaries",
+ ] + start_binaries,
+ srcs = [
+ "//aos:prime_binaries",
+ ] + binaries,
+ dirs = dirs,
+ default_target = default_target,
+ restricted_to = ["//tools:roborio"],
+ )
- aos_downloader(
- name = 'download_stripped',
- start_srcs = [
- '//aos:prime_start_binaries_stripped',
- ] + [expand_label(binary) + ".stripped" for binary in start_binaries],
- srcs = [
- '//aos:prime_binaries_stripped',
- ] + [expand_label(binary) + ".stripped" for binary in binaries],
- dirs = dirs,
- default_target = default_target,
- restricted_to = ['//tools:roborio'],
- )
+ aos_downloader(
+ name = "download_stripped",
+ start_srcs = [
+ "//aos:prime_start_binaries_stripped",
+ ] + [expand_label(binary) + ".stripped" for binary in start_binaries],
+ srcs = [
+ "//aos:prime_binaries_stripped",
+ ] + [expand_label(binary) + ".stripped" for binary in binaries],
+ dirs = dirs,
+ default_target = default_target,
+ restricted_to = ["//tools:roborio"],
+ )
diff --git a/frc971/downloader/BUILD b/frc971/downloader/BUILD
new file mode 100644
index 0000000..1aab653
--- /dev/null
+++ b/frc971/downloader/BUILD
@@ -0,0 +1,12 @@
+py_binary(
+ name = "downloader",
+ srcs = [
+ "downloader.py",
+ ],
+ data = [
+ "@rsync",
+ "@ssh",
+ "@ssh//:scp",
+ ],
+ visibility = ["//visibility:public"],
+)
diff --git a/frc971/downloader/downloader.bzl b/frc971/downloader/downloader.bzl
new file mode 100644
index 0000000..a5d1bc1
--- /dev/null
+++ b/frc971/downloader/downloader.bzl
@@ -0,0 +1,119 @@
+def _aos_downloader_impl(ctx):
+ all_files = ctx.files.srcs + ctx.files.start_srcs + [ctx.outputs._startlist]
+ ctx.file_action(
+ output = ctx.outputs.executable,
+ executable = True,
+ content = "\n".join([
+ "#!/bin/bash",
+ "set -e",
+ 'cd "${BASH_SOURCE[0]}.runfiles/%s"' % ctx.workspace_name,
+ ] + ['%s %s --dirs %s -- %s "$@"' % (
+ ctx.executable._downloader.short_path,
+ " ".join([src.short_path for src in d.downloader_srcs]),
+ d.downloader_dir,
+ ctx.attr.default_target,
+ ) for d in ctx.attr.dirs] + [
+ 'exec %s %s -- %s "$@"' % (
+ ctx.executable._downloader.short_path,
+ " ".join([src.short_path for src in all_files]),
+ ctx.attr.default_target,
+ ),
+ ]),
+ )
+
+ ctx.file_action(
+ output = ctx.outputs._startlist,
+ content = "\n".join([f.basename for f in ctx.files.start_srcs]) + "\n",
+ )
+
+ to_download = [ctx.outputs._startlist]
+ to_download += all_files
+ for d in ctx.attr.dirs:
+ to_download += d.downloader_srcs
+
+ return struct(
+ runfiles = ctx.runfiles(
+ files = to_download + ctx.files._downloader,
+ transitive_files = ctx.attr._downloader.default_runfiles.files,
+ collect_data = True,
+ collect_default = True,
+ ),
+ files = depset([ctx.outputs.executable]),
+ )
+
+def _aos_downloader_dir_impl(ctx):
+ return struct(
+ downloader_dir = ctx.attr.dir,
+ downloader_srcs = ctx.files.srcs,
+ )
+
+"""Creates a binary which downloads code to a robot.
+
+Running this with `bazel run` will actually download everything.
+
+This also generates a start_list.txt file with the names of binaries to start.
+
+Attrs:
+ srcs: The files to download. They currently all get shoved into one folder.
+ dirs: A list of aos_downloader_dirs to download too.
+ start_srcs: Like srcs, except they also get put into start_list.txt.
+ default_target: The default host to download to. If not specified, defaults to
+ roboRIO-971.local.
+"""
+
+aos_downloader = rule(
+ attrs = {
+ "_downloader": attr.label(
+ executable = True,
+ cfg = "host",
+ default = Label("//frc971/downloader"),
+ ),
+ "start_srcs": attr.label_list(
+ mandatory = True,
+ allow_files = True,
+ ),
+ "srcs": attr.label_list(
+ mandatory = True,
+ allow_files = True,
+ ),
+ "dirs": attr.label_list(
+ mandatory = False,
+ providers = [
+ "downloader_dir",
+ "downloader_srcs",
+ ],
+ ),
+ "default_target": attr.string(
+ default = "roboRIO-971-frc.local",
+ ),
+ },
+ executable = True,
+ outputs = {
+ "_startlist": "%{name}.start_list.dir/start_list.txt",
+ },
+ implementation = _aos_downloader_impl,
+)
+
+"""Downloads files to a specific directory.
+
+This rule does nothing by itself. Use it by adding to the dirs attribute of an
+aos_downloader rule.
+
+Attrs:
+ srcs: The files to download. They all go in the same directory.
+ dir: The directory (relative to the standard download directory) to put all
+ the files in.
+"""
+
+aos_downloader_dir = rule(
+ attrs = {
+ "srcs": attr.label_list(
+ mandatory = True,
+ allow_files = True,
+ ),
+ "dir": attr.string(
+ mandatory = True,
+ ),
+ },
+ implementation = _aos_downloader_dir_impl,
+)
diff --git a/frc971/downloader/downloader.py b/frc971/downloader/downloader.py
new file mode 100644
index 0000000..e49ee5d
--- /dev/null
+++ b/frc971/downloader/downloader.py
@@ -0,0 +1,94 @@
+# This file is run by shell scripts generated by the aos_downloader Skylark
+# macro. Everything before the first -- is a hard-coded list of files to
+# download.
+
+from __future__ import print_function
+
+import sys
+import subprocess
+import re
+import os
+
+
+def install(ssh_target, pkg):
+ """Installs a package from NI on the ssh target."""
+ print("Installing", pkg)
+ PKG_URL = "http://download.ni.com/ni-linux-rt/feeds/2015/arm/ipk/cortexa9-vfpv3/" + pkg
+ subprocess.check_call(["wget", PKG_URL, "-O", pkg])
+ try:
+ subprocess.check_call([
+ "external/ssh/usr/bin/scp", "-S", "external/ssh/usr/bin/ssh", pkg,
+ ssh_target + ":/tmp/" + pkg
+ ])
+ subprocess.check_call([
+ "external/ssh/usr/bin/ssh", ssh_target, "opkg", "install",
+ "/tmp/" + pkg
+ ])
+ subprocess.check_call(
+ ["external/ssh/usr/bin/ssh", ssh_target, "rm", "/tmp/" + pkg])
+ finally:
+ subprocess.check_call(["rm", pkg])
+
+
+def main(argv):
+ args = argv[argv.index("--") + 1:]
+
+ relative_dir = ""
+ recursive = False
+
+ if "--dirs" in argv:
+ dirs_index = argv.index("--dirs")
+ srcs = argv[1:dirs_index]
+ relative_dir = argv[dirs_index + 1]
+ recursive = True
+ else:
+ srcs = argv[1:argv.index("--")]
+
+ ROBORIO_TARGET_DIR = "/home/admin/robot_code"
+ ROBORIO_USER = "admin"
+
+ target_dir = ROBORIO_TARGET_DIR
+ user = ROBORIO_USER
+ destination = args[-1]
+
+ result = re.match("(?:([^:@]+)@)?([^:@]+)(?::([^:@]+))?", destination)
+ if not result:
+ print(
+ "Not sure how to parse destination \"%s\"!" % destination,
+ file=sys.stderr)
+ return 1
+ if result.group(1):
+ user = result.group(1)
+ hostname = result.group(2)
+ if result.group(3):
+ target_dir = result.group(3)
+
+ ssh_target = "%s@%s" % (user, hostname)
+
+ rsync_cmd = ([
+ "external/rsync/usr/bin/rsync", "-e", "external/ssh/usr/bin/ssh", "-c",
+ "-v", "-z", "--copy-links"
+ ] + srcs + ["%s:%s/%s" % (ssh_target, target_dir, relative_dir)])
+ try:
+ subprocess.check_call(rsync_cmd)
+ except subprocess.CalledProcessError as e:
+ if e.returncode == 127:
+ print("Unconfigured roboRIO, installing rsync.")
+ install(ssh_target, "libattr1_2.4.47-r0.36_cortexa9-vfpv3.ipk")
+ install(ssh_target, "libacl1_2.2.52-r0.36_cortexa9-vfpv3.ipk")
+ install(ssh_target, "rsync_3.1.0-r0.7_cortexa9-vfpv3.ipk")
+ subprocess.check_call(rsync_cmd)
+ else:
+ raise e
+
+ if not recursive:
+ subprocess.check_call(
+ ("external/ssh/usr/bin/ssh", ssh_target, "&&".join([
+ "chmod u+s %s/starter_exe" % target_dir,
+ "echo \'Done moving new executables into place\'",
+ "bash -c \'sync && sync && sync\'",
+ ])))
+
+
+if __name__ == "__main__":
+ main(sys.argv)
diff --git a/frc971/queues/BUILD b/frc971/queues/BUILD
index 83ee252..e440638 100644
--- a/frc971/queues/BUILD
+++ b/frc971/queues/BUILD
@@ -1,10 +1,11 @@
-package(default_visibility = ['//visibility:public'])
+package(default_visibility = ["//visibility:public"])
-load('//aos/build:queues.bzl', 'queue_library')
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
-queue_library(
- name = 'gyro',
- srcs = [
- 'gyro.q',
- ],
+flatbuffer_cc_library(
+ name = "gyro",
+ srcs = [
+ "gyro.fbs",
+ ],
+ gen_reflections = 1,
)
diff --git a/frc971/queues/gyro.fbs b/frc971/queues/gyro.fbs
new file mode 100644
index 0000000..8a76678
--- /dev/null
+++ b/frc971/queues/gyro.fbs
@@ -0,0 +1,18 @@
+namespace frc971.sensors;
+
+// Published on "/drivetrain"
+table GyroReading {
+ // Positive is counter-clockwise (Austin says "it's Positive").
+ // Right-hand coordinate system around the Z-axis going up.
+ // The angle is measured in radians.
+ angle:double;
+ // The angular velocity in radians/sec
+ velocity:double;
+}
+
+// Published on "/drivetrain"
+table Uid {
+ uid:uint;
+}
+
+root_type GyroReading;
diff --git a/frc971/queues/gyro.q b/frc971/queues/gyro.q
deleted file mode 100644
index 0885738..0000000
--- a/frc971/queues/gyro.q
+++ /dev/null
@@ -1,16 +0,0 @@
-package frc971.sensors;
-
-// Published on ".frc971.sensors.gyro_reading"
-message GyroReading {
- // Positive is counter-clockwise (Austin says "it's Positive").
- // Right-hand coordinate system around the Z-axis going up.
- // The angle is measured in radians.
- double angle;
- // The angular velocity in radians/sec
- double velocity;
-};
-
-// Published on ".frc971.sensors.gyro_part_id"
-message Uid {
- uint32_t uid;
-};
diff --git a/frc971/wpilib/ADIS16448.cc b/frc971/wpilib/ADIS16448.cc
index 4fc2c7b..f364d56 100644
--- a/frc971/wpilib/ADIS16448.cc
+++ b/frc971/wpilib/ADIS16448.cc
@@ -8,10 +8,9 @@
#include <chrono>
#include "aos/init.h"
-#include "aos/logging/queue_logging.h"
-#include "aos/robot_state/robot_state.q.h"
+#include "aos/robot_state/robot_state_generated.h"
#include "aos/time/time.h"
-#include "frc971/wpilib/imu.q.h"
+#include "frc971/wpilib/imu_generated.h"
#include "frc971/zeroing/averager.h"
namespace frc971 {
@@ -121,10 +120,9 @@
ADIS16448::ADIS16448(::aos::EventLoop *event_loop, frc::SPI::Port port,
frc::DigitalInput *dio1)
: event_loop_(event_loop),
- joystick_state_fetcher_(event_loop_->MakeFetcher<::aos::JoystickState>(
- ".aos.joystick_state")),
+ robot_state_fetcher_(event_loop_->MakeFetcher<::aos::RobotState>("/aos")),
imu_values_sender_(
- event_loop_->MakeSender<::frc971::IMUValues>(".frc971.imu_values")),
+ event_loop_->MakeSender<::frc971::IMUValues>("/drivetrain")),
spi_(new frc::SPI(port)),
dio1_(dio1) {
// 1MHz is the maximum supported for burst reads, but we
@@ -247,31 +245,35 @@
}
}
- auto message = imu_values_sender_.MakeMessage();
- message->fpga_timestamp = ::aos::time::DurationInSeconds(
- dio1_->ReadRisingTimestamp().time_since_epoch());
- message->monotonic_timestamp_ns =
- chrono::duration_cast<chrono::nanoseconds>(read_time.time_since_epoch())
- .count();
+ auto builder = imu_values_sender_.MakeBuilder();
- message->gyro_x =
+ IMUValues::Builder imu_builder = builder.MakeBuilder<IMUValues>();
+
+ imu_builder.add_fpga_timestamp(::aos::time::DurationInSeconds(
+ dio1_->ReadRisingTimestamp().time_since_epoch()));
+ imu_builder.add_monotonic_timestamp_ns(
+ chrono::duration_cast<chrono::nanoseconds>(read_time.time_since_epoch())
+ .count());
+
+ float gyro_x =
ConvertValue(&to_receive[4], kGyroLsbDegreeSecond * M_PI / 180.0);
- message->gyro_y =
+ float gyro_y =
ConvertValue(&to_receive[6], kGyroLsbDegreeSecond * M_PI / 180.0);
- message->gyro_z =
+ float gyro_z =
ConvertValue(&to_receive[8], kGyroLsbDegreeSecond * M_PI / 180.0);
// The first few seconds of samples are averaged and subtracted from
// subsequent samples for zeroing purposes.
if (!gyros_are_zeroed_) {
- average_gyro_x.AddData(message->gyro_x);
- average_gyro_y.AddData(message->gyro_y);
- average_gyro_z.AddData(message->gyro_z);
+ average_gyro_x.AddData(gyro_x);
+ average_gyro_y.AddData(gyro_y);
+ average_gyro_z.AddData(gyro_z);
if (average_gyro_x.full() && average_gyro_y.full() &&
average_gyro_z.full()) {
- joystick_state_fetcher_.Fetch();
- if (joystick_state_fetcher_.get() && joystick_state_fetcher_->enabled) {
+ robot_state_fetcher_.Fetch();
+ if (robot_state_fetcher_.get() &&
+ robot_state_fetcher_->outputs_enabled()) {
gyro_x_zeroed_offset_ = -average_gyro_x.GetAverage();
gyro_y_zeroed_offset_ = -average_gyro_y.GetAverage();
gyro_z_zeroed_offset_ = -average_gyro_z.GetAverage();
@@ -282,33 +284,36 @@
}
}
}
+ gyro_x += gyro_x_zeroed_offset_;
+ gyro_y += gyro_y_zeroed_offset_;
+ gyro_z += gyro_z_zeroed_offset_;
- message->gyro_x += gyro_x_zeroed_offset_;
- message->gyro_y += gyro_y_zeroed_offset_;
- message->gyro_z += gyro_z_zeroed_offset_;
+ imu_builder.add_gyro_x(gyro_x);
+ imu_builder.add_gyro_y(gyro_y);
+ imu_builder.add_gyro_z(gyro_z);
- message->accelerometer_x =
- ConvertValue(&to_receive[10], kAccelerometerLsbG);
- message->accelerometer_y =
- ConvertValue(&to_receive[12], kAccelerometerLsbG);
- message->accelerometer_z =
- ConvertValue(&to_receive[14], kAccelerometerLsbG);
+ imu_builder.add_accelerometer_x(
+ ConvertValue(&to_receive[10], kAccelerometerLsbG));
+ imu_builder.add_accelerometer_y(
+ ConvertValue(&to_receive[12], kAccelerometerLsbG));
+ imu_builder.add_accelerometer_z(
+ ConvertValue(&to_receive[14], kAccelerometerLsbG));
- message->magnetometer_x =
- ConvertValue(&to_receive[16], kMagnetometerLsbGauss);
- message->magnetometer_y =
- ConvertValue(&to_receive[18], kMagnetometerLsbGauss);
- message->magnetometer_z =
- ConvertValue(&to_receive[20], kMagnetometerLsbGauss);
+ imu_builder.add_magnetometer_x(
+ ConvertValue(&to_receive[16], kMagnetometerLsbGauss));
+ imu_builder.add_magnetometer_y(
+ ConvertValue(&to_receive[18], kMagnetometerLsbGauss));
+ imu_builder.add_magnetometer_z(
+ ConvertValue(&to_receive[20], kMagnetometerLsbGauss));
- message->barometer =
- ConvertValue(&to_receive[22], kBarometerLsbPascal, false);
+ imu_builder.add_barometer(
+ ConvertValue(&to_receive[22], kBarometerLsbPascal, false));
- message->temperature =
- ConvertValue(&to_receive[24], kTemperatureLsbDegree) + kTemperatureZero;
+ imu_builder.add_temperature(
+ ConvertValue(&to_receive[24], kTemperatureLsbDegree) +
+ kTemperatureZero);
- AOS_LOG_STRUCT(DEBUG, "sending", *message);
- if (!message.Send()) {
+ if (!builder.Send(imu_builder.Finish())) {
AOS_LOG(WARNING, "sending queue message failed\n");
}
diff --git a/frc971/wpilib/ADIS16448.h b/frc971/wpilib/ADIS16448.h
index 6aeac9d..ce950df 100644
--- a/frc971/wpilib/ADIS16448.h
+++ b/frc971/wpilib/ADIS16448.h
@@ -11,10 +11,10 @@
#include "frc971/wpilib/ahal/SPI.h"
#undef ERROR
-#include "aos/events/event-loop.h"
+#include "aos/events/event_loop.h"
#include "aos/logging/logging.h"
-#include "aos/robot_state/robot_state.q.h"
-#include "frc971/wpilib/imu.q.h"
+#include "aos/robot_state/robot_state_generated.h"
+#include "frc971/wpilib/imu_generated.h"
#include "frc971/wpilib/spi_rx_clearer.h"
namespace frc971 {
@@ -90,7 +90,7 @@
bool Initialize();
::aos::EventLoop *event_loop_;
- ::aos::Fetcher<::aos::JoystickState> joystick_state_fetcher_;
+ ::aos::Fetcher<::aos::RobotState> robot_state_fetcher_;
::aos::Sender<::frc971::IMUValues> imu_values_sender_;
// TODO(Brian): This object has no business owning these ones.
diff --git a/frc971/wpilib/BUILD b/frc971/wpilib/BUILD
index 870e655..b7fe837 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -1,11 +1,11 @@
package(default_visibility = ["//visibility:public"])
-load("//aos/build:queues.bzl", "queue_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
-queue_library(
- name = "logging_queue",
+flatbuffer_cc_library(
+ name = "logging_fbs",
srcs = [
- "logging.q",
+ "logging.fbs",
],
)
@@ -107,10 +107,9 @@
deps = [
":gyro_interface",
"//aos:init",
- "//aos/events:event-loop",
+ "//aos/events:event_loop",
"//aos/logging",
- "//aos/logging:queue_logging",
- "//aos/robot_state",
+ "//aos/robot_state:robot_state_fbs",
"//aos/time",
"//aos/util:phased_loop",
"//frc971/queues:gyro",
@@ -118,10 +117,10 @@
],
)
-queue_library(
- name = "loop_output_handler_test_queue",
+flatbuffer_cc_library(
+ name = "loop_output_handler_test_fbs",
srcs = [
- "loop_output_handler_test.q",
+ "loop_output_handler_test.fbs",
],
)
@@ -135,8 +134,8 @@
],
deps = [
"//aos:init",
- "//aos/events:event-loop",
- "//aos/robot_state",
+ "//aos/events:event_loop",
+ "//aos/robot_state:robot_state_fbs",
"//aos/scoped:scoped_fd",
"//aos/time",
"//aos/util:log_interval",
@@ -150,9 +149,8 @@
],
deps = [
":loop_output_handler",
- ":loop_output_handler_test_queue",
+ ":loop_output_handler_test_fbs",
"//aos/events:simulated_event_loop",
- "//aos/logging:queue_logging",
"//aos/testing:googletest",
"//aos/testing:test_logging",
],
@@ -169,10 +167,10 @@
restricted_to = ["//tools:roborio"],
deps = [
"//aos:init",
- "//aos/events:shm-event-loop",
- "//aos/logging:queue_logging",
+ "//aos/events:shm_event_loop",
+ "//aos/input:driver_station_data",
"//aos/network:team_number",
- "//aos/robot_state",
+ "//aos/robot_state:joystick_state_fbs",
"//third_party:wpilib",
],
)
@@ -187,16 +185,16 @@
],
restricted_to = ["//tools:roborio"],
deps = [
- "//aos/logging:queue_logging",
- "//aos/robot_state",
+ "//aos/events:event_loop",
+ "//aos/robot_state:robot_state_fbs",
"//third_party:wpilib",
],
)
-queue_library(
+flatbuffer_cc_library(
name = "pdp_values",
srcs = [
- "pdp_values.q",
+ "pdp_values.fbs",
],
)
@@ -212,8 +210,7 @@
deps = [
":pdp_values",
"//aos:init",
- "//aos/events:event-loop",
- "//aos/logging:queue_logging",
+ "//aos/events:event_loop",
"//aos/util:phased_loop",
"//third_party:wpilib",
],
@@ -230,11 +227,12 @@
],
)
-queue_library(
- name = "imu_queue",
+flatbuffer_cc_library(
+ name = "imu_fbs",
srcs = [
- "imu.q",
+ "imu.fbs",
],
+ gen_reflections = 1,
)
cc_library(
@@ -247,13 +245,12 @@
],
restricted_to = ["//tools:roborio"],
deps = [
- ":imu_queue",
+ ":imu_fbs",
":spi_rx_clearer",
"//aos:init",
- "//aos/events:event-loop",
+ "//aos/events:event_loop",
"//aos/logging",
- "//aos/logging:queue_logging",
- "//aos/robot_state",
+ "//aos/robot_state:robot_state_fbs",
"//aos/time",
"//frc971/zeroing:averager",
"//third_party:wpilib",
@@ -302,11 +299,11 @@
":encoder_and_potentiometer",
":wpilib_interface",
"//aos:init",
- "//aos/events:event-loop",
+ "//aos/events:event_loop",
"//aos/stl_mutex",
"//aos/time",
"//aos/util:phased_loop",
- "//frc971/control_loops:queues",
+ "//frc971/control_loops:control_loops_fbs",
"//third_party:wpilib",
],
)
@@ -324,8 +321,7 @@
":loop_output_handler",
"//aos:math",
"//aos/logging",
- "//aos/logging:queue_logging",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//frc971/control_loops/drivetrain:drivetrain_output_fbs",
"//third_party:wpilib",
],
)
diff --git a/frc971/wpilib/drivetrain_writer.cc b/frc971/wpilib/drivetrain_writer.cc
index 5c6feb8..0afbe20 100644
--- a/frc971/wpilib/drivetrain_writer.cc
+++ b/frc971/wpilib/drivetrain_writer.cc
@@ -2,8 +2,7 @@
#include "aos/commonmath.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
#include "frc971/wpilib/ahal/PWM.h"
#include "frc971/wpilib/loop_output_handler.h"
@@ -11,19 +10,19 @@
namespace wpilib {
void DrivetrainWriter::Write(
- const ::frc971::control_loops::DrivetrainQueue::Output &output) {
- AOS_LOG_STRUCT(DEBUG, "will output", output);
- left_controller0_->SetSpeed(SafeSpeed(reversed_left0_, output.left_voltage));
+ const ::frc971::control_loops::drivetrain::Output &output) {
+ left_controller0_->SetSpeed(
+ SafeSpeed(reversed_left0_, output.left_voltage()));
right_controller0_->SetSpeed(
- SafeSpeed(reversed_right0_, output.right_voltage));
+ SafeSpeed(reversed_right0_, output.right_voltage()));
if (left_controller1_) {
left_controller1_->SetSpeed(
- SafeSpeed(reversed_left1_, output.left_voltage));
+ SafeSpeed(reversed_left1_, output.left_voltage()));
}
if (right_controller1_) {
right_controller1_->SetSpeed(
- SafeSpeed(reversed_right1_, output.right_voltage));
+ SafeSpeed(reversed_right1_, output.right_voltage()));
}
}
diff --git a/frc971/wpilib/drivetrain_writer.h b/frc971/wpilib/drivetrain_writer.h
index 9397230..9cf9a67 100644
--- a/frc971/wpilib/drivetrain_writer.h
+++ b/frc971/wpilib/drivetrain_writer.h
@@ -3,7 +3,7 @@
#include "aos/commonmath.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
#include "frc971/wpilib/ahal/PWM.h"
#include "frc971/wpilib/loop_output_handler.h"
@@ -11,12 +11,12 @@
namespace wpilib {
class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler<
- ::frc971::control_loops::DrivetrainQueue::Output> {
+ ::frc971::control_loops::drivetrain::Output> {
public:
DrivetrainWriter(::aos::EventLoop *event_loop)
: ::frc971::wpilib::LoopOutputHandler<
- ::frc971::control_loops::DrivetrainQueue::Output>(
- event_loop, ".frc971.control_loops.drivetrain_queue.output") {}
+ ::frc971::control_loops::drivetrain::Output>(event_loop,
+ "/drivetrain") {}
void set_left_controller0(::std::unique_ptr<::frc::PWM> t, bool reversed) {
left_controller0_ = ::std::move(t);
@@ -40,12 +40,11 @@
private:
void Write(
- const ::frc971::control_loops::DrivetrainQueue::Output &output) override;
+ const ::frc971::control_loops::drivetrain::Output &output) override;
void Stop() override;
double SafeSpeed(bool reversed, double voltage) {
- return (::aos::Clip((reversed ? -1.0 : 1.0) * voltage, -12.0, 12.0) /
- 12.0);
+ return (::aos::Clip((reversed ? -1.0 : 1.0) * voltage, -12.0, 12.0) / 12.0);
}
::std::unique_ptr<::frc::PWM> left_controller0_, right_controller0_,
diff --git a/frc971/wpilib/encoder_and_potentiometer.cc b/frc971/wpilib/encoder_and_potentiometer.cc
index ac1a4a0..8173d20 100644
--- a/frc971/wpilib/encoder_and_potentiometer.cc
+++ b/frc971/wpilib/encoder_and_potentiometer.cc
@@ -1,7 +1,7 @@
#include "frc971/wpilib/encoder_and_potentiometer.h"
-#include "aos/init.h"
#include "aos/logging/logging.h"
+#include "aos/realtime.h"
namespace frc971 {
namespace wpilib {
diff --git a/frc971/wpilib/gyro_sender.cc b/frc971/wpilib/gyro_sender.cc
index 3a9f220..f5297c0 100644
--- a/frc971/wpilib/gyro_sender.cc
+++ b/frc971/wpilib/gyro_sender.cc
@@ -7,14 +7,13 @@
#include <chrono>
-#include "aos/events/event-loop.h"
+#include "aos/events/event_loop.h"
#include "aos/init.h"
#include "aos/logging/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 "aos/time/time.h"
-#include "frc971/queues/gyro.q.h"
+#include "frc971/queues/gyro_generated.h"
#include "frc971/zeroing/averager.h"
namespace frc971 {
@@ -25,13 +24,12 @@
GyroSender::GyroSender(::aos::EventLoop *event_loop)
: event_loop_(event_loop),
- joystick_state_fetcher_(event_loop_->MakeFetcher<::aos::JoystickState>(
- ".aos.joystick_state")),
- uid_sender_(event_loop_->MakeSender<::frc971::sensors::Uid>(
- ".frc971.sensors.gyro_part_id")),
+ joystick_state_fetcher_(
+ event_loop_->MakeFetcher<aos::RobotState>("/aos")),
+ uid_sender_(event_loop_->MakeSender<frc971::sensors::Uid>("/drivetrain")),
gyro_reading_sender_(
- event_loop_->MakeSender<::frc971::sensors::GyroReading>(
- ".frc971.sensors.gyro_reading")) {
+ event_loop_->MakeSender<frc971::sensors::GyroReading>(
+ "/drivetrain")) {
AOS_PCHECK(
system("ps -ef | grep '\\[spi0\\]' | awk '{print $1}' | xargs chrt -f -p "
"33") == 0);
@@ -55,12 +53,9 @@
state_ = State::RUNNING;
AOS_LOG(INFO, "gyro initialized successfully\n");
- {
- auto message = uid_sender_.MakeMessage();
- message->uid = gyro_.ReadPartID();
- AOS_LOG_STRUCT(INFO, "gyro ID", *message);
- message.Send();
- }
+ auto builder = uid_sender_.MakeBuilder();
+ builder.Send(
+ frc971::sensors::CreateUid(*builder.fbb(), gyro_.ReadPartID()));
}
last_initialize_time_ = monotonic_now;
}
@@ -115,30 +110,31 @@
const double angle_rate = gyro_.ExtractAngle(result);
const double new_angle = angle_rate / static_cast<double>(kReadingRate);
- auto message = gyro_reading_sender_.MakeMessage();
+ auto builder = gyro_reading_sender_.MakeBuilder();
if (zeroed_) {
angle_ += (new_angle + zero_offset_) * iterations;
- message->angle = angle_;
- message->velocity = angle_rate + zero_offset_ * kReadingRate;
- AOS_LOG_STRUCT(DEBUG, "sending", *message);
- message.Send();
+ sensors::GyroReading::Builder gyro_builder =
+ builder.MakeBuilder<sensors::GyroReading>();
+ gyro_builder.add_angle(angle_);
+ gyro_builder.add_velocity(angle_rate + zero_offset_ * kReadingRate);
+ builder.Send(gyro_builder.Finish());
} else {
// TODO(brian): Don't break without 6 seconds of standing still before
// enabling. Ideas:
// Don't allow driving until we have at least some data?
// Some kind of indicator light?
{
- message->angle = new_angle;
- message->velocity = angle_rate;
- AOS_LOG_STRUCT(DEBUG, "collected while zeroing", *message);
- message->angle = 0.0;
- message->velocity = 0.0;
- message.Send();
+ sensors::GyroReading::Builder gyro_builder =
+ builder.MakeBuilder<sensors::GyroReading>();
+ gyro_builder.add_angle(0.0);
+ gyro_builder.add_velocity(0.0);
+ builder.Send(gyro_builder.Finish());
}
zeroing_data_.AddData(new_angle);
joystick_state_fetcher_.Fetch();
- if (joystick_state_fetcher_.get() && joystick_state_fetcher_->enabled &&
+ if (joystick_state_fetcher_.get() &&
+ joystick_state_fetcher_->outputs_enabled() &&
zeroing_data_.full()) {
zero_offset_ = -zeroing_data_.GetAverage();
AOS_LOG(INFO, "total zero offset %f\n", zero_offset_);
diff --git a/frc971/wpilib/gyro_sender.h b/frc971/wpilib/gyro_sender.h
index 3a43391..ec39264 100644
--- a/frc971/wpilib/gyro_sender.h
+++ b/frc971/wpilib/gyro_sender.h
@@ -5,9 +5,9 @@
#include <atomic>
-#include "aos/events/event-loop.h"
-#include "aos/robot_state/robot_state.q.h"
-#include "frc971/queues/gyro.q.h"
+#include "aos/events/event_loop.h"
+#include "aos/robot_state/robot_state_generated.h"
+#include "frc971/queues/gyro_generated.h"
#include "frc971/wpilib/gyro_interface.h"
#include "frc971/zeroing/averager.h"
@@ -30,7 +30,7 @@
void Loop(const int iterations);
::aos::EventLoop *event_loop_;
- ::aos::Fetcher<::aos::JoystickState> joystick_state_fetcher_;
+ ::aos::Fetcher<::aos::RobotState> joystick_state_fetcher_;
::aos::Sender<::frc971::sensors::Uid> uid_sender_;
::aos::Sender<::frc971::sensors::GyroReading> gyro_reading_sender_;
diff --git a/frc971/wpilib/imu.q b/frc971/wpilib/imu.fbs
similarity index 69%
rename from frc971/wpilib/imu.q
rename to frc971/wpilib/imu.fbs
index 4d9dec7..f48f31f 100644
--- a/frc971/wpilib/imu.q
+++ b/frc971/wpilib/imu.fbs
@@ -1,40 +1,42 @@
-package frc971;
+namespace frc971;
// Values returned from an IMU.
// Published on ".frc971.imu_values"
-message IMUValues {
+table IMUValues {
// Gyro readings in radians/second.
// Positive is clockwise looking at the connector.
- float gyro_x;
+ gyro_x:float;
// Positive is clockwise looking at the right side (from the connector).
- float gyro_y;
+ gyro_y:float;
// Positive is counterclockwise looking at the top.
- float gyro_z;
+ gyro_z:float;
// Accelerometer readings in Gs.
// Positive is up.
- float accelerometer_x;
+ accelerometer_x:float;
// Positive is away from the right side (from the connector).
- float accelerometer_y;
+ accelerometer_y:float;
// Positive is away from the connector.
- float accelerometer_z;
+ accelerometer_z:float;
// Magnetometer readings in gauss.
// Positive is up.
- float magnetometer_x;
+ magnetometer_x:float;
// Positive is away from the right side (from the connector).
- float magnetometer_y;
+ magnetometer_y:float;
// Positive is away from the connector.
- float magnetometer_z;
+ magnetometer_z:float;
// Barometer readings in pascals.
- float barometer;
+ barometer:float;
// Temperature readings in degrees Celsius.
- float temperature;
+ temperature:float;
// FPGA timestamp when the values were captured.
- double fpga_timestamp;
+ fpga_timestamp:double;
// CLOCK_MONOTONIC time in nanoseconds when the values were captured.
- int64_t monotonic_timestamp_ns;
-};
+ monotonic_timestamp_ns:long;
+}
+
+root_type IMUValues;
diff --git a/frc971/wpilib/interrupt_edge_counting.cc b/frc971/wpilib/interrupt_edge_counting.cc
index 546e64b..9b63b84 100644
--- a/frc971/wpilib/interrupt_edge_counting.cc
+++ b/frc971/wpilib/interrupt_edge_counting.cc
@@ -2,8 +2,8 @@
#include <chrono>
+#include "aos/realtime.h"
#include "aos/time/time.h"
-#include "aos/init.h"
namespace frc971 {
namespace wpilib {
diff --git a/frc971/wpilib/joystick_sender.cc b/frc971/wpilib/joystick_sender.cc
index 31ecc6c..2fec111 100644
--- a/frc971/wpilib/joystick_sender.cc
+++ b/frc971/wpilib/joystick_sender.cc
@@ -1,9 +1,10 @@
#include "frc971/wpilib/joystick_sender.h"
-#include "aos/init.h"
-#include "aos/logging/queue_logging.h"
+#include "aos/input/driver_station_data.h"
+#include "aos/logging/logging.h"
#include "aos/network/team_number.h"
-#include "aos/robot_state/robot_state.q.h"
+#include "aos/realtime.h"
+#include "aos/robot_state/joystick_state_generated.h"
#include "frc971/wpilib/ahal/DriverStation.h"
#include "hal/HAL.h"
@@ -11,12 +12,13 @@
namespace frc971 {
namespace wpilib {
+using aos::Joystick;
+
JoystickSender::JoystickSender(::aos::EventLoop *event_loop)
: event_loop_(event_loop),
joystick_state_sender_(
- event_loop_->MakeSender<::aos::JoystickState>(".aos.joystick_state")),
+ event_loop_->MakeSender<::aos::JoystickState>("/aos")),
team_id_(::aos::network::GetTeamNumber()) {
-
event_loop_->SetRuntimeRealtimePriority(29);
event_loop_->OnRun([this]() {
@@ -27,36 +29,65 @@
// variable / mutex needs to get exposed all the way out or something).
while (event_loop_->is_running()) {
ds->RunIteration([&]() {
- auto new_state = joystick_state_sender_.MakeMessage();
+ auto builder = joystick_state_sender_.MakeBuilder();
HAL_MatchInfo match_info;
auto status = HAL_GetMatchInfo(&match_info);
- if (status == 0) {
- new_state->switch_left = match_info.gameSpecificMessage[0] == 'L' ||
- match_info.gameSpecificMessage[0] == 'l';
- new_state->scale_left = match_info.gameSpecificMessage[1] == 'L' ||
- match_info.gameSpecificMessage[1] == 'l';
- }
- new_state->test_mode = ds->IsTestMode();
- new_state->fms_attached = ds->IsFmsAttached();
- new_state->enabled = ds->IsEnabled();
- new_state->autonomous = ds->IsAutonomous();
- new_state->team_id = team_id_;
- new_state->fake = false;
+ std::array<flatbuffers::Offset<Joystick>,
+ aos::input::driver_station::JoystickFeature::kJoysticks>
+ joysticks;
- for (uint8_t i = 0;
- i < sizeof(new_state->joysticks) / sizeof(::aos::Joystick); ++i) {
- new_state->joysticks[i].buttons = ds->GetStickButtons(i);
- for (int j = 0; j < 6; ++j) {
- new_state->joysticks[i].axis[j] = ds->GetStickAxis(i, j);
+ for (size_t i = 0;
+ i < aos::input::driver_station::JoystickFeature::kJoysticks; ++i) {
+ std::array<double, aos::input::driver_station::JoystickAxis::kAxes>
+ axis;
+ for (int j = 0; j < aos::input::driver_station::JoystickAxis::kAxes;
+ ++j) {
+ axis[j] = ds->GetStickAxis(i, j);
}
+
+ flatbuffers::Offset<flatbuffers::Vector<double>> axis_offset =
+ builder.fbb()->CreateVector(axis.begin(), axis.size());
+
+ Joystick::Builder joystick_builder = builder.MakeBuilder<Joystick>();
+
+ joystick_builder.add_buttons(ds->GetStickButtons(i));
+
if (ds->GetStickPOVCount(i) > 0) {
- new_state->joysticks[i].pov = ds->GetStickPOV(i, 0);
+ joystick_builder.add_pov(ds->GetStickPOV(i, 0));
}
- AOS_LOG_STRUCT(DEBUG, "joystick_state", *new_state);
+
+ joystick_builder.add_axis(axis_offset);
+
+ joysticks[i] = joystick_builder.Finish();
}
- if (!new_state.Send()) {
+
+ flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<Joystick>>>
+ joysticks_offset = builder.fbb()->CreateVector(joysticks.begin(),
+ joysticks.size());
+
+ aos::JoystickState::Builder joystick_state_builder =
+ builder.MakeBuilder<aos::JoystickState>();
+
+ joystick_state_builder.add_joysticks(joysticks_offset);
+
+ if (status == 0) {
+ joystick_state_builder.add_switch_left(
+ match_info.gameSpecificMessage[0] == 'L' ||
+ match_info.gameSpecificMessage[0] == 'l');
+ joystick_state_builder.add_scale_left(
+ match_info.gameSpecificMessage[1] == 'L' ||
+ match_info.gameSpecificMessage[1] == 'l');
+ }
+
+ joystick_state_builder.add_test_mode(ds->IsTestMode());
+ joystick_state_builder.add_fms_attached(ds->IsFmsAttached());
+ joystick_state_builder.add_enabled(ds->IsEnabled());
+ joystick_state_builder.add_autonomous(ds->IsAutonomous());
+ joystick_state_builder.add_team_id(team_id_);
+
+ if (!builder.Send(joystick_state_builder.Finish())) {
AOS_LOG(WARNING, "sending joystick_state failed\n");
}
});
diff --git a/frc971/wpilib/joystick_sender.h b/frc971/wpilib/joystick_sender.h
index 34c6bf4..e2609e8 100644
--- a/frc971/wpilib/joystick_sender.h
+++ b/frc971/wpilib/joystick_sender.h
@@ -3,8 +3,8 @@
#include <atomic>
-#include "aos/events/event-loop.h"
-#include "aos/robot_state/robot_state.q.h"
+#include "aos/events/event_loop.h"
+#include "aos/robot_state/joystick_state_generated.h"
namespace frc971 {
namespace wpilib {
diff --git a/frc971/wpilib/logging.fbs b/frc971/wpilib/logging.fbs
new file mode 100644
index 0000000..473526f
--- /dev/null
+++ b/frc971/wpilib/logging.fbs
@@ -0,0 +1,7 @@
+namespace frc971.wpilib;
+
+// Information about the current state of the pneumatics system to log.
+table PneumaticsToLog {
+ compressor_on:bool;
+ read_solenoids:ubyte;
+}
diff --git a/frc971/wpilib/logging.q b/frc971/wpilib/logging.q
deleted file mode 100644
index a2b3799..0000000
--- a/frc971/wpilib/logging.q
+++ /dev/null
@@ -1,7 +0,0 @@
-package frc971.wpilib;
-
-// Information about the current state of the pneumatics system to log.
-struct PneumaticsToLog {
- bool compressor_on;
- uint8_t read_solenoids;
-};
diff --git a/frc971/wpilib/loop_output_handler.h b/frc971/wpilib/loop_output_handler.h
index 81baa68..a697339 100644
--- a/frc971/wpilib/loop_output_handler.h
+++ b/frc971/wpilib/loop_output_handler.h
@@ -4,8 +4,7 @@
#include <atomic>
#include <chrono>
-#include "aos/events/event-loop.h"
-#include "aos/robot_state/robot_state.q.h"
+#include "aos/events/event_loop.h"
#include "aos/scoped/scoped_fd.h"
#include "aos/time/time.h"
#include "aos/util/log_interval.h"
diff --git a/frc971/wpilib/loop_output_handler_test.cc b/frc971/wpilib/loop_output_handler_test.cc
index 12c3f96..d7ef8cd 100644
--- a/frc971/wpilib/loop_output_handler_test.cc
+++ b/frc971/wpilib/loop_output_handler_test.cc
@@ -4,12 +4,11 @@
#include "gtest/gtest.h"
-#include "aos/events/simulated-event-loop.h"
+#include "aos/events/simulated_event_loop.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
#include "aos/testing/test_logging.h"
#include "aos/time/time.h"
-#include "frc971/wpilib/loop_output_handler_test.q.h"
+#include "frc971/wpilib/loop_output_handler_test_generated.h"
namespace frc971 {
namespace wpilib {
@@ -23,11 +22,26 @@
public:
LoopOutputHandlerTest()
: ::testing::Test(),
+ configuration_(aos::configuration::MergeConfiguration(
+ aos::FlatbufferDetachedBuffer<aos::Configuration>(
+ aos::JsonToFlatbuffer(
+ "{\n"
+ " \"channels\": [ \n"
+ " {\n"
+ " \"name\": \"/test\",\n"
+ " \"type\": "
+ "\"frc971.wpilib.LoopOutputHandlerTestOutput\"\n"
+ " }\n"
+ " ]\n"
+ "}\n",
+ aos::Configuration::MiniReflectTypeTable())))),
+ event_loop_factory_(&configuration_.message()),
loop_output_hander_event_loop_(event_loop_factory_.MakeEventLoop()),
test_event_loop_(event_loop_factory_.MakeEventLoop()) {
::aos::testing::EnableTestLogging();
}
+ aos::FlatbufferDetachedBuffer<aos::Configuration> configuration_;
::aos::SimulatedEventLoopFactory event_loop_factory_;
::std::unique_ptr<::aos::EventLoop> loop_output_hander_event_loop_;
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
@@ -49,14 +63,14 @@
protected:
void Write(const LoopOutputHandlerTestOutput &output) override {
- AOS_LOG_STRUCT(DEBUG, "output", output);
+ LOG(INFO) << "output " << aos::FlatbufferToJson(&output);
++count_;
last_time_ = event_loop()->monotonic_now();
}
void Stop() override {
stop_time_ = event_loop()->monotonic_now();
- AOS_LOG(DEBUG, "Stopping\n");
+ LOG(INFO) << "Stopping";
}
private:
@@ -71,10 +85,10 @@
// Test that the watchdog calls Stop at the right time.
TEST_F(LoopOutputHandlerTest, WatchdogTest) {
TestLoopOutputHandler loop_output(loop_output_hander_event_loop_.get(),
- ".test");
+ "/test");
::aos::Sender<LoopOutputHandlerTestOutput> output_sender =
- test_event_loop_->MakeSender<LoopOutputHandlerTestOutput>(".test");
+ test_event_loop_->MakeSender<LoopOutputHandlerTestOutput>("/test");
const monotonic_clock::time_point start_time =
test_event_loop_->monotonic_now();
@@ -86,13 +100,15 @@
EXPECT_EQ(count, loop_output.count());
if (test_event_loop_->monotonic_now() <
start_time + chrono::seconds(1)) {
- auto output = output_sender.MakeMessage();
- output->voltage = 5.0;
- EXPECT_TRUE(output.Send());
+ auto builder = output_sender.MakeBuilder();
+ LoopOutputHandlerTestOutput::Builder output_builder =
+ builder.MakeBuilder<LoopOutputHandlerTestOutput>();
+ output_builder.add_voltage(5.0);
+ EXPECT_TRUE(builder.Send(output_builder.Finish()));
++count;
}
- AOS_LOG(INFO, "Ping\n");
+ LOG(INFO) << "Ping";
});
// Kick off the ping timer handler.
diff --git a/frc971/wpilib/loop_output_handler_test.fbs b/frc971/wpilib/loop_output_handler_test.fbs
new file mode 100644
index 0000000..6cb2cf1
--- /dev/null
+++ b/frc971/wpilib/loop_output_handler_test.fbs
@@ -0,0 +1,6 @@
+namespace frc971.wpilib;
+
+// Test output message.
+table LoopOutputHandlerTestOutput {
+ voltage:double;
+}
diff --git a/frc971/wpilib/loop_output_handler_test.q b/frc971/wpilib/loop_output_handler_test.q
deleted file mode 100644
index 81336ef..0000000
--- a/frc971/wpilib/loop_output_handler_test.q
+++ /dev/null
@@ -1,6 +0,0 @@
-package frc971.wpilib;
-
-// Test output message.
-message LoopOutputHandlerTestOutput {
- double voltage;
-};
diff --git a/frc971/wpilib/pdp_fetcher.cc b/frc971/wpilib/pdp_fetcher.cc
index aa85184..cd17d89 100644
--- a/frc971/wpilib/pdp_fetcher.cc
+++ b/frc971/wpilib/pdp_fetcher.cc
@@ -2,11 +2,10 @@
#include <chrono>
-#include "aos/events/event-loop.h"
-#include "aos/init.h"
-#include "aos/logging/queue_logging.h"
+#include "aos/events/event_loop.h"
+#include "aos/logging/logging.h"
#include "frc971/wpilib/ahal/PowerDistributionPanel.h"
-#include "frc971/wpilib/pdp_values.q.h"
+#include "frc971/wpilib/pdp_values_generated.h"
namespace frc971 {
namespace wpilib {
@@ -15,8 +14,7 @@
PDPFetcher::PDPFetcher(::aos::EventLoop *event_loop)
: event_loop_(event_loop),
- pdp_values_sender_(
- event_loop_->MakeSender<::frc971::PDPValues>(".frc971.pdp_values")),
+ pdp_values_sender_(event_loop_->MakeSender<::frc971::PDPValues>("/aos")),
pdp_(new frc::PowerDistributionPanel()) {
event_loop_->set_name("PDPFetcher");
@@ -31,15 +29,22 @@
if (iterations != 1) {
AOS_LOG(DEBUG, "PDPFetcher skipped %d iterations\n", iterations - 1);
}
- auto message = pdp_values_sender_.MakeMessage();
- message->voltage = pdp_->GetVoltage();
- message->temperature = pdp_->GetTemperature();
- message->power = pdp_->GetTotalPower();
- for (int i = 0; i < 16; ++i) {
- message->currents[i] = pdp_->GetCurrent(i);
+ std::array<double, 16> currents;
+ for (size_t i = 0; i < currents.size(); ++i) {
+ currents[i] = pdp_->GetCurrent(i);
}
- AOS_LOG_STRUCT(DEBUG, "got", *message);
- if (!message.Send()) {
+
+ auto builder = pdp_values_sender_.MakeBuilder();
+ flatbuffers::Offset<flatbuffers::Vector<double>> currents_offset =
+ builder.fbb()->CreateVector(currents.begin(), currents.size());
+
+ PDPValues::Builder pdp_builder = builder.MakeBuilder<PDPValues>();
+ pdp_builder.add_voltage(pdp_->GetVoltage());
+ pdp_builder.add_temperature(pdp_->GetTemperature());
+ pdp_builder.add_power(pdp_->GetTotalPower());
+ pdp_builder.add_currents(currents_offset);
+
+ if (!builder.Send(pdp_builder.Finish())) {
AOS_LOG(WARNING, "sending pdp values failed\n");
}
}
diff --git a/frc971/wpilib/pdp_fetcher.h b/frc971/wpilib/pdp_fetcher.h
index fd05d67..d034473 100644
--- a/frc971/wpilib/pdp_fetcher.h
+++ b/frc971/wpilib/pdp_fetcher.h
@@ -4,8 +4,8 @@
#include <atomic>
#include <memory>
-#include "aos/events/event-loop.h"
-#include "frc971/wpilib/pdp_values.q.h"
+#include "aos/events/event_loop.h"
+#include "frc971/wpilib/pdp_values_generated.h"
namespace frc {
class PowerDistributionPanel;
diff --git a/frc971/wpilib/pdp_values.fbs b/frc971/wpilib/pdp_values.fbs
new file mode 100644
index 0000000..4db2ade
--- /dev/null
+++ b/frc971/wpilib/pdp_values.fbs
@@ -0,0 +1,13 @@
+namespace frc971;
+
+// Values retrieved from the PDP.
+// Published on ".frc971.pdp_values"
+table PDPValues {
+ voltage:double;
+ temperature:double;
+ power:double;
+ // Array of 16 currents.
+ currents:[double];
+}
+
+root_type PDPValues;
diff --git a/frc971/wpilib/pdp_values.q b/frc971/wpilib/pdp_values.q
deleted file mode 100644
index 4c326ab..0000000
--- a/frc971/wpilib/pdp_values.q
+++ /dev/null
@@ -1,10 +0,0 @@
-package frc971;
-
-// Values retrieved from the PDP.
-// Published on ".frc971.pdp_values"
-message PDPValues {
- double voltage;
- double temperature;
- double power;
- double[16] currents;
-};
diff --git a/frc971/wpilib/sensor_reader.cc b/frc971/wpilib/sensor_reader.cc
index 63d3992..8610105 100644
--- a/frc971/wpilib/sensor_reader.cc
+++ b/frc971/wpilib/sensor_reader.cc
@@ -4,7 +4,6 @@
#include <unistd.h>
#include "aos/init.h"
-#include "aos/logging/queue_logging.h"
#include "aos/util/compiler_memory_barrier.h"
#include "aos/util/phased_loop.h"
#include "frc971/wpilib/ahal/DigitalInput.h"
@@ -124,10 +123,8 @@
event_loop_->monotonic_now();
{
- auto new_state = robot_state_sender_.MakeMessage();
- ::frc971::wpilib::PopulateRobotState(new_state.get(), my_pid_);
- AOS_LOG_STRUCT(DEBUG, "robot_state", *new_state);
- new_state.Send();
+ auto builder = robot_state_sender_.MakeBuilder();
+ builder.Send(::frc971::wpilib::PopulateRobotState(&builder, my_pid_));
}
RunIteration();
if (dma_synchronizer_) {
diff --git a/frc971/wpilib/sensor_reader.h b/frc971/wpilib/sensor_reader.h
index 7a63444..ae1d580 100644
--- a/frc971/wpilib/sensor_reader.h
+++ b/frc971/wpilib/sensor_reader.h
@@ -4,11 +4,11 @@
#include <atomic>
#include <chrono>
-#include "aos/events/event-loop.h"
-#include "aos/robot_state/robot_state.q.h"
+#include "aos/events/event_loop.h"
+#include "aos/robot_state/robot_state_generated.h"
#include "aos/stl_mutex/stl_mutex.h"
#include "aos/time/time.h"
-#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/wpilib/ahal/DigitalGlitchFilter.h"
#include "frc971/wpilib/ahal/DigitalInput.h"
#include "frc971/wpilib/ahal/DriverStation.h"
@@ -63,7 +63,7 @@
// Copies a DMAEncoder to a IndexPosition with the correct unit and direction
// changes.
void CopyPosition(const ::frc971::wpilib::DMAEncoder &encoder,
- ::frc971::IndexPosition *position,
+ ::frc971::IndexPositionT *position,
double encoder_counts_per_revolution, double encoder_ratio,
bool reverse) {
const double multiplier = reverse ? -1.0 : 1.0;
@@ -82,7 +82,7 @@
// the correct unit and direction changes.
void CopyPosition(
const ::frc971::wpilib::AbsoluteEncoderAndPotentiometer &encoder,
- ::frc971::PotAndAbsolutePosition *position,
+ ::frc971::PotAndAbsolutePositionT *position,
double encoder_counts_per_revolution, double encoder_ratio,
::std::function<double(double)> potentiometer_translate, bool reverse,
double pot_offset) {
@@ -104,7 +104,7 @@
// Copies a DMAEdgeCounter to a HallEffectAndPosition with the correct unit
// and direction changes.
void CopyPosition(const ::frc971::wpilib::DMAEdgeCounter &counter,
- ::frc971::HallEffectAndPosition *position,
+ ::frc971::HallEffectAndPositionT *position,
double encoder_counts_per_revolution, double encoder_ratio,
bool reverse) {
const double multiplier = reverse ? -1.0 : 1.0;
@@ -129,7 +129,7 @@
// and direction changes.
void CopyPosition(
const ::frc971::wpilib::AbsoluteEncoder &encoder,
- ::frc971::AbsolutePosition *position,
+ ::frc971::AbsolutePositionT *position,
double encoder_counts_per_revolution, double encoder_ratio,
bool reverse) {
const double multiplier = reverse ? -1.0 : 1.0;
@@ -146,7 +146,7 @@
void CopyPosition(
const ::frc971::wpilib::DMAEncoderAndPotentiometer &encoder,
- ::frc971::PotAndIndexPosition *position,
+ ::frc971::PotAndIndexPositionT *position,
::std::function<double(int32_t)> encoder_translate,
::std::function<double(double)> potentiometer_translate, bool reverse,
double pot_offset) {
diff --git a/frc971/wpilib/wpilib_interface.cc b/frc971/wpilib/wpilib_interface.cc
index 4216e3b..8772af1 100644
--- a/frc971/wpilib/wpilib_interface.cc
+++ b/frc971/wpilib/wpilib_interface.cc
@@ -1,30 +1,38 @@
#include "frc971/wpilib/wpilib_interface.h"
-#include "aos/robot_state/robot_state.q.h"
+#include "aos/events/event_loop.h"
+#include "aos/logging/logging.h"
+#include "aos/robot_state/robot_state_generated.h"
#include "hal/HAL.h"
namespace frc971 {
namespace wpilib {
-void PopulateRobotState(::aos::RobotState *robot_state, int32_t my_pid) {
+flatbuffers::Offset<aos::RobotState> PopulateRobotState(
+ aos::Sender<::aos::RobotState>::Builder *builder, int32_t my_pid) {
int32_t status = 0;
- robot_state->reader_pid = my_pid;
- robot_state->outputs_enabled = HAL_GetSystemActive(&status);
- robot_state->browned_out = HAL_GetBrownedOut(&status);
+ aos::RobotState::Builder robot_state_builder =
+ builder->MakeBuilder<aos::RobotState>();
- robot_state->is_3v3_active = HAL_GetUserActive3V3(&status);
- robot_state->is_5v_active = HAL_GetUserActive5V(&status);
- robot_state->voltage_3v3 = HAL_GetUserVoltage3V3(&status);
- robot_state->voltage_5v = HAL_GetUserVoltage5V(&status);
+ robot_state_builder.add_reader_pid(my_pid);
+ robot_state_builder.add_outputs_enabled(HAL_GetSystemActive(&status));
+ robot_state_builder.add_browned_out(HAL_GetBrownedOut(&status));
- robot_state->voltage_roborio_in = HAL_GetVinVoltage(&status);
- robot_state->voltage_battery = HAL_GetVinVoltage(&status);
+ robot_state_builder.add_is_3v3_active(HAL_GetUserActive3V3(&status));
+ robot_state_builder.add_is_5v_active(HAL_GetUserActive5V(&status));
+ robot_state_builder.add_voltage_3v3(HAL_GetUserVoltage3V3(&status));
+ robot_state_builder.add_voltage_5v(HAL_GetUserVoltage5V(&status));
+
+ robot_state_builder.add_voltage_roborio_in(HAL_GetVinVoltage(&status));
+ robot_state_builder.add_voltage_battery(HAL_GetVinVoltage(&status));
if (status != 0) {
AOS_LOG(FATAL, "Failed to get robot state: %d\n", status);
}
+
+ return robot_state_builder.Finish();
}
} // namespace wpilib
diff --git a/frc971/wpilib/wpilib_interface.h b/frc971/wpilib/wpilib_interface.h
index 5db852a..104ab84 100644
--- a/frc971/wpilib/wpilib_interface.h
+++ b/frc971/wpilib/wpilib_interface.h
@@ -3,13 +3,15 @@
#include <stdint.h>
-#include "aos/robot_state/robot_state.q.h"
+#include "aos/events/event_loop.h"
+#include "aos/robot_state/robot_state_generated.h"
namespace frc971 {
namespace wpilib {
// Sends out a message on ::aos::robot_state.
-void PopulateRobotState(::aos::RobotState *robot_state, int32_t my_pid);
+flatbuffers::Offset<aos::RobotState> PopulateRobotState(
+ aos::Sender<::aos::RobotState>::Builder *builder, int32_t my_pid);
} // namespace wpilib
} // namespace frc971
diff --git a/frc971/wpilib/wpilib_robot_base.h b/frc971/wpilib/wpilib_robot_base.h
index 86672c4..978c48e 100644
--- a/frc971/wpilib/wpilib_robot_base.h
+++ b/frc971/wpilib/wpilib_robot_base.h
@@ -1,8 +1,9 @@
#ifndef FRC971_WPILIB_NEWROBOTBASE_H_
#define FRC971_WPILIB_NEWROBOTBASE_H_
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
+#include "aos/logging/logging.h"
#include "frc971/wpilib/ahal/RobotBase.h"
namespace frc971 {
diff --git a/frc971/zeroing/BUILD b/frc971/zeroing/BUILD
index db5507b..fe95bdc 100644
--- a/frc971/zeroing/BUILD
+++ b/frc971/zeroing/BUILD
@@ -1,6 +1,5 @@
package(default_visibility = ["//visibility:public"])
-load("//aos/build:queues.bzl", "queue_library")
load("//tools:environments.bzl", "mcu_cpus")
cc_library(
@@ -31,8 +30,10 @@
],
deps = [
":wrap",
+ "//aos/logging",
"//frc971:constants",
- "//frc971/control_loops:queues",
+ "//frc971/control_loops:control_loops_fbs",
+ "@com_github_google_glog//:glog",
],
)
@@ -44,11 +45,10 @@
deps = [
":zeroing",
"//aos:die",
- "//aos/util:thread",
"//aos/testing:googletest",
"//aos/testing:test_shm",
+ "//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:position_sensor_sim",
- "//frc971/control_loops:queues",
],
)
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index 1399efd..f0aab23 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -8,6 +8,9 @@
#include "frc971/zeroing/wrap.h"
+#include "flatbuffers/flatbuffers.h"
+#include "glog/logging.h"
+
namespace frc971 {
namespace zeroing {
@@ -30,7 +33,7 @@
void PotAndIndexPulseZeroingEstimator::TriggerError() {
if (!error_) {
- AOS_LOG(ERROR, "Manually triggered zeroing error.\n");
+ VLOG(1) << "Manually triggered zeroing error.";
error_ = true;
}
}
@@ -57,14 +60,14 @@
// reset and wait for that count to change before we consider ourselves
// zeroed.
if (wait_for_index_pulse_) {
- last_used_index_pulse_count_ = info.index_pulses;
+ last_used_index_pulse_count_ = info.index_pulses();
wait_for_index_pulse_ = false;
}
if (start_pos_samples_.size() < constants_.average_filter_size) {
- start_pos_samples_.push_back(info.pot - info.encoder);
+ start_pos_samples_.push_back(info.pot() - info.encoder());
} else {
- start_pos_samples_[samples_idx_] = info.pot - info.encoder;
+ start_pos_samples_[samples_idx_] = info.pot() - info.encoder();
}
// Drop the oldest sample when we run this function the next time around.
@@ -83,14 +86,14 @@
// have a well-filtered starting position then we use the filtered value as
// our best guess.
if (!zeroed_ &&
- (info.index_pulses == last_used_index_pulse_count_ || !offset_ready())) {
+ (info.index_pulses() == last_used_index_pulse_count_ || !offset_ready())) {
offset_ = start_average;
- } else if (!zeroed_ || last_used_index_pulse_count_ != info.index_pulses) {
+ } else if (!zeroed_ || last_used_index_pulse_count_ != info.index_pulses()) {
// Note the accurate start position and the current index pulse count so
// that we only run this logic once per index pulse. That should be more
// resilient to corrupted intermediate data.
- offset_ = CalculateStartPosition(start_average, info.latched_encoder);
- last_used_index_pulse_count_ = info.index_pulses;
+ offset_ = CalculateStartPosition(start_average, info.latched_encoder());
+ last_used_index_pulse_count_ = info.index_pulses();
// TODO(austin): Reject encoder positions which have x% error rather than
// rounding to the closest index pulse.
@@ -98,7 +101,7 @@
// Save the first starting position.
if (!zeroed_) {
first_start_pos_ = offset_;
- AOS_LOG(INFO, "latching start position %f\n", first_start_pos_);
+ VLOG(2) << "latching start position" << first_start_pos_;
}
// Now that we have an accurate starting position we can consider ourselves
@@ -109,29 +112,30 @@
if (::std::abs(first_start_pos_ - offset_) >
constants_.allowable_encoder_error * constants_.index_difference) {
if (!error_) {
- AOS_LOG(
- ERROR,
- "Encoder ticks out of range since last index pulse. first start "
- "position: %f recent starting position: %f, allowable error: %f\n",
- first_start_pos_, offset_,
- constants_.allowable_encoder_error * constants_.index_difference);
+ VLOG(1)
+ << "Encoder ticks out of range since last index pulse. first start "
+ "position: "
+ << first_start_pos_ << " recent starting position: " << offset_
+ << ", allowable error: "
+ << constants_.allowable_encoder_error * constants_.index_difference;
error_ = true;
}
}
}
- position_ = offset_ + info.encoder;
- filtered_position_ = start_average + info.encoder;
+ position_ = offset_ + info.encoder();
+ filtered_position_ = start_average + info.encoder();
}
-PotAndIndexPulseZeroingEstimator::State
-PotAndIndexPulseZeroingEstimator::GetEstimatorState() const {
- State r;
- r.error = error_;
- r.zeroed = zeroed_;
- r.position = position_;
- r.pot_position = filtered_position_;
- return r;
+flatbuffers::Offset<PotAndIndexPulseZeroingEstimator::State>
+PotAndIndexPulseZeroingEstimator::GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const {
+ State::Builder builder(*fbb);
+ builder.add_error(error_);
+ builder.add_zeroed(zeroed_);
+ builder.add_position(position_);
+ builder.add_pot_position(filtered_position_);
+ return builder.Finish();
}
HallEffectAndPositionZeroingEstimator::HallEffectAndPositionZeroingEstimator(
@@ -157,7 +161,7 @@
void HallEffectAndPositionZeroingEstimator::TriggerError() {
if (!error_) {
- AOS_LOG(ERROR, "Manually triggered zeroing error.\n");
+ VLOG(1) << "Manually triggered zeroing error.\n";
error_ = true;
}
}
@@ -165,15 +169,15 @@
void HallEffectAndPositionZeroingEstimator::StoreEncoderMaxAndMin(
const HallEffectAndPosition &info) {
// If we have a new posedge.
- if (!info.current) {
+ if (!info.current()) {
if (last_hall_) {
- min_low_position_ = max_low_position_ = info.encoder;
+ min_low_position_ = max_low_position_ = info.encoder();
} else {
- min_low_position_ = ::std::min(min_low_position_, info.encoder);
- max_low_position_ = ::std::max(max_low_position_, info.encoder);
+ min_low_position_ = ::std::min(min_low_position_, info.encoder());
+ max_low_position_ = ::std::max(max_low_position_, info.encoder());
}
}
- last_hall_ = info.current;
+ last_hall_ = info.current();
}
void HallEffectAndPositionZeroingEstimator::UpdateEstimate(
@@ -183,49 +187,49 @@
// that count to change and for the hall effect to stay high before we
// consider ourselves zeroed.
if (!initialized_) {
- last_used_posedge_count_ = info.posedge_count;
+ last_used_posedge_count_ = info.posedge_count();
initialized_ = true;
- last_hall_ = info.current;
+ last_hall_ = info.current();
}
StoreEncoderMaxAndMin(info);
- if (info.current) {
+ if (info.current()) {
cycles_high_++;
} else {
cycles_high_ = 0;
- last_used_posedge_count_ = info.posedge_count;
+ last_used_posedge_count_ = info.posedge_count();
}
high_long_enough_ = cycles_high_ >= constants_.hall_trigger_zeroing_length;
bool moving_backward = false;
if (constants_.zeroing_move_direction) {
- moving_backward = info.encoder > min_low_position_;
+ moving_backward = info.encoder() > min_low_position_;
} else {
- moving_backward = info.encoder < max_low_position_;
+ moving_backward = info.encoder() < max_low_position_;
}
// If there are no posedges to use or we don't have enough samples yet to
// have a well-filtered starting position then we use the filtered value as
// our best guess.
- if (last_used_posedge_count_ != info.posedge_count && high_long_enough_ &&
+ if (last_used_posedge_count_ != info.posedge_count() && high_long_enough_ &&
moving_backward) {
// Note the offset and the current posedge count so that we only run this
// logic once per posedge. That should be more resilient to corrupted
// intermediate data.
- offset_ = -info.posedge_value;
+ offset_ = -info.posedge_value();
if (constants_.zeroing_move_direction) {
offset_ += constants_.lower_hall_position;
} else {
offset_ += constants_.upper_hall_position;
}
- last_used_posedge_count_ = info.posedge_count;
+ last_used_posedge_count_ = info.posedge_count();
// Save the first starting position.
if (!zeroed_) {
first_start_pos_ = offset_;
- AOS_LOG(INFO, "latching start position %f\n", first_start_pos_);
+ VLOG(2) << "latching start position" << first_start_pos_;
}
// Now that we have an accurate starting position we can consider ourselves
@@ -233,18 +237,19 @@
zeroed_ = true;
}
- position_ = info.encoder - offset_;
+ position_ = info.encoder() - offset_;
}
-HallEffectAndPositionZeroingEstimator::State
-HallEffectAndPositionZeroingEstimator::GetEstimatorState() const {
- State r;
- r.error = error_;
- r.zeroed = zeroed_;
- r.encoder = position_;
- r.high_long_enough = high_long_enough_;
- r.offset = offset_;
- return r;
+flatbuffers::Offset<HallEffectAndPositionZeroingEstimator::State>
+HallEffectAndPositionZeroingEstimator::GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const {
+ State::Builder builder(*fbb);
+ builder.add_error(error_);
+ builder.add_zeroed(zeroed_);
+ builder.add_encoder(position_);
+ builder.add_high_long_enough(high_long_enough_);
+ builder.add_offset(offset_);
+ return builder.Finish();
}
PotAndAbsoluteEncoderZeroingEstimator::PotAndAbsoluteEncoderZeroingEstimator(
@@ -291,23 +296,22 @@
const PotAndAbsolutePosition &info) {
// Check for Abs Encoder NaN value that would mess up the rest of the zeroing
// code below. NaN values are given when the Absolute Encoder is disconnected.
- if (::std::isnan(info.absolute_encoder)) {
+ if (::std::isnan(info.absolute_encoder())) {
if (zeroed_) {
- AOS_LOG(ERROR, "NAN on absolute encoder\n");
+ VLOG(1) << "NAN on absolute encoder.";
error_ = true;
} else {
++nan_samples_;
- AOS_LOG(ERROR, "NAN on absolute encoder while zeroing %d\n",
- static_cast<int>(nan_samples_));
+ VLOG(1) << "NAN on absolute encoder while zeroing" << nan_samples_;
if (nan_samples_ >= constants_.average_filter_size) {
error_ = true;
zeroed_ = true;
}
}
// Throw some dummy values in for now.
- filtered_absolute_encoder_ = info.absolute_encoder;
- filtered_position_ = pot_relative_encoder_offset_ + info.encoder;
- position_ = offset_ + info.encoder;
+ filtered_absolute_encoder_ = info.absolute_encoder();
+ filtered_position_ = pot_relative_encoder_offset_ + info.encoder();
+ position_ = offset_ + info.encoder();
return;
}
@@ -315,7 +319,7 @@
constants_.zeroing_threshold);
if (!moving) {
- const PotAndAbsolutePosition &sample = move_detector_.GetSample();
+ const PositionStruct &sample = move_detector_.GetSample();
// Compute the average offset between the absolute encoder and relative
// encoder. If we have 0 samples, assume it is 0.
@@ -402,13 +406,10 @@
if (::std::abs(first_offset_ - offset_) >
constants_.allowable_encoder_error *
constants_.one_revolution_distance) {
- AOS_LOG(
- ERROR,
- "Offset moved too far. Initial: %f, current %f, allowable change: "
- "%f\n",
- first_offset_, offset_,
- constants_.allowable_encoder_error *
- constants_.one_revolution_distance);
+ VLOG(1) << "Offset moved too far. Initial: " << first_offset_
+ << ", current " << offset_ << ", allowable change: "
+ << constants_.allowable_encoder_error *
+ constants_.one_revolution_distance;
error_ = true;
}
@@ -417,19 +418,20 @@
}
// Update the position.
- filtered_position_ = pot_relative_encoder_offset_ + info.encoder;
- position_ = offset_ + info.encoder;
+ filtered_position_ = pot_relative_encoder_offset_ + info.encoder();
+ position_ = offset_ + info.encoder();
}
-PotAndAbsoluteEncoderZeroingEstimator::State
-PotAndAbsoluteEncoderZeroingEstimator::GetEstimatorState() const {
- State r;
- r.error = error_;
- r.zeroed = zeroed_;
- r.position = position_;
- r.pot_position = filtered_position_;
- r.absolute_position = filtered_absolute_encoder_;
- return r;
+flatbuffers::Offset<PotAndAbsoluteEncoderZeroingEstimator::State>
+PotAndAbsoluteEncoderZeroingEstimator::GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const {
+ State::Builder builder(*fbb);
+ builder.add_error(error_);
+ builder.add_zeroed(zeroed_);
+ builder.add_position(position_);
+ builder.add_pot_position(filtered_position_);
+ builder.add_absolute_position(filtered_absolute_encoder_);
+ return builder.Finish();
}
void PulseIndexZeroingEstimator::Reset() {
@@ -444,16 +446,16 @@
void PulseIndexZeroingEstimator::StoreIndexPulseMaxAndMin(
const IndexPosition &info) {
// If we have a new index pulse.
- if (last_used_index_pulse_count_ != info.index_pulses) {
+ if (last_used_index_pulse_count_ != info.index_pulses()) {
// If the latest pulses's position is outside the range we've currently
// seen, record it appropriately.
- if (info.latched_encoder > max_index_position_) {
- max_index_position_ = info.latched_encoder;
+ if (info.latched_encoder() > max_index_position_) {
+ max_index_position_ = info.latched_encoder();
}
- if (info.latched_encoder < min_index_position_) {
- min_index_position_ = info.latched_encoder;
+ if (info.latched_encoder() < min_index_position_) {
+ min_index_position_ = info.latched_encoder();
}
- last_used_index_pulse_count_ = info.index_pulses;
+ last_used_index_pulse_count_ = info.index_pulses();
}
}
@@ -474,9 +476,9 @@
const int index_pulse_count = IndexPulseCount();
if (index_pulse_count > constants_.index_pulse_count) {
if (!error_) {
- AOS_LOG(ERROR,
- "Got more index pulses than expected. Got %d expected %d.\n",
- index_pulse_count, constants_.index_pulse_count);
+ VLOG(1) << "Got more index pulses than expected. Got "
+ << index_pulse_count << " expected "
+ << constants_.index_pulse_count;
error_ = true;
}
}
@@ -493,7 +495,7 @@
// Detect whether the index pulse is somewhere other than where we expect
// it to be. First we compute the position of the most recent index pulse.
double index_pulse_distance =
- info.latched_encoder + offset_ - constants_.measured_index_position;
+ info.latched_encoder() + offset_ - constants_.measured_index_position;
// Second we compute the position of the index pulse in terms of
// the index difference. I.e. if this index pulse is two pulses away from
// the index pulse that we know about then this number should be positive
@@ -506,32 +508,34 @@
// This lets us check if the index pulse is within an acceptable error
// margin of where we expected it to be.
if (::std::abs(error) > constants_.allowable_encoder_error) {
- AOS_LOG(ERROR,
- "Encoder ticks out of range since last index pulse. known index "
- "pulse: %f, expected index pulse: %f, actual index pulse: %f, "
- "allowable error: %f\n",
- constants_.measured_index_position,
- round(relative_distance) * constants_.index_difference +
- constants_.measured_index_position,
- info.latched_encoder + offset_,
- constants_.allowable_encoder_error * constants_.index_difference);
+ VLOG(1)
+ << "Encoder ticks out of range since last index pulse. known index "
+ "pulse: "
+ << constants_.measured_index_position << ", expected index pulse: "
+ << round(relative_distance) * constants_.index_difference +
+ constants_.measured_index_position
+ << ", actual index pulse: " << info.latched_encoder() + offset_
+ << ", "
+ "allowable error: "
+ << constants_.allowable_encoder_error * constants_.index_difference;
error_ = true;
}
}
- position_ = info.encoder + offset_;
+ position_ = info.encoder() + offset_;
}
-PulseIndexZeroingEstimator::State
-PulseIndexZeroingEstimator::GetEstimatorState() const {
- State r;
- r.error = error_;
- r.zeroed = zeroed_;
- r.position = position_;
- r.min_index_position = min_index_position_;
- r.max_index_position = max_index_position_;
- r.index_pulses_seen = IndexPulseCount();
- return r;
+flatbuffers::Offset<PulseIndexZeroingEstimator::State>
+PulseIndexZeroingEstimator::GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const {
+ State::Builder builder(*fbb);
+ builder.add_error(error_);
+ builder.add_zeroed(zeroed_);
+ builder.add_position(position_);
+ builder.add_min_index_position(min_index_position_);
+ builder.add_max_index_position(max_index_position_);
+ builder.add_index_pulses_seen(IndexPulseCount());
+ return builder.Finish();
}
AbsoluteEncoderZeroingEstimator::AbsoluteEncoderZeroingEstimator(
@@ -570,22 +574,21 @@
const AbsolutePosition &info) {
// Check for Abs Encoder NaN value that would mess up the rest of the zeroing
// code below. NaN values are given when the Absolute Encoder is disconnected.
- if (::std::isnan(info.absolute_encoder)) {
+ if (::std::isnan(info.absolute_encoder())) {
if (zeroed_) {
- AOS_LOG(ERROR, "NAN on absolute encoder\n");
+ VLOG(1) << "NAN on absolute encoder.";
error_ = true;
} else {
++nan_samples_;
- AOS_LOG(ERROR, "NAN on absolute encoder while zeroing %d\n",
- static_cast<int>(nan_samples_));
+ VLOG(1) << "NAN on absolute encoder while zeroing " << nan_samples_;
if (nan_samples_ >= constants_.average_filter_size) {
error_ = true;
zeroed_ = true;
}
}
// Throw some dummy values in for now.
- filtered_absolute_encoder_ = info.absolute_encoder;
- position_ = offset_ + info.encoder;
+ filtered_absolute_encoder_ = info.absolute_encoder();
+ position_ = offset_ + info.encoder();
return;
}
@@ -593,7 +596,7 @@
constants_.zeroing_threshold);
if (!moving) {
- const AbsolutePosition &sample = move_detector_.GetSample();
+ const PositionStruct &sample = move_detector_.GetSample();
// Compute the average offset between the absolute encoder and relative
// encoder. If we have 0 samples, assume it is 0.
@@ -673,13 +676,10 @@
if (::std::abs(first_offset_ - offset_) >
constants_.allowable_encoder_error *
constants_.one_revolution_distance) {
- AOS_LOG(
- ERROR,
- "Offset moved too far. Initial: %f, current %f, allowable change: "
- "%f\n",
- first_offset_, offset_,
- constants_.allowable_encoder_error *
- constants_.one_revolution_distance);
+ VLOG(1) << "Offset moved too far. Initial: " << first_offset_
+ << ", current " << offset_ << ", allowable change: "
+ << constants_.allowable_encoder_error *
+ constants_.one_revolution_distance;
error_ = true;
}
@@ -688,17 +688,18 @@
}
// Update the position.
- position_ = offset_ + info.encoder;
+ position_ = offset_ + info.encoder();
}
-AbsoluteEncoderZeroingEstimator::State
- AbsoluteEncoderZeroingEstimator::GetEstimatorState() const {
- State r;
- r.error = error_;
- r.zeroed = zeroed_;
- r.position = position_;
- r.absolute_position = filtered_absolute_encoder_;
- return r;
+flatbuffers::Offset<AbsoluteEncoderZeroingEstimator::State>
+AbsoluteEncoderZeroingEstimator::GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const {
+ State::Builder builder(*fbb);
+ builder.add_error(error_);
+ builder.add_zeroed(zeroed_);
+ builder.add_position(position_);
+ builder.add_absolute_position(filtered_absolute_encoder_);
+ return builder.Finish();
}
} // namespace zeroing
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index e0c6169..1c6bb77 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -6,9 +6,11 @@
#include <cstdint>
#include <vector>
-#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/constants.h"
+#include "flatbuffers/flatbuffers.h"
+
// TODO(pschrader): Flag an error if encoder index pulse is not n revolutions
// away from the last one (i.e. got extra counts from noise, etc..)
//
@@ -53,7 +55,8 @@
virtual void UpdateEstimate(const Position &) = 0;
// Returns the state of the estimator
- virtual State GetEstimatorState() const = 0;
+ virtual flatbuffers::Offset<State> GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const = 0;
};
// Estimates the position with an incremental encoder with an index pulse and a
@@ -98,7 +101,8 @@
}
// Returns information about our current state.
- State GetEstimatorState() const override;
+ virtual flatbuffers::Offset<State> GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const override;
private:
// This function calculates the start position given the internal state and
@@ -168,7 +172,8 @@
bool offset_ready() const override { return zeroed_; }
// Returns information about our current state.
- State GetEstimatorState() const override;
+ virtual flatbuffers::Offset<State> GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const override;
private:
// Sets the minimum and maximum posedge position values.
@@ -217,7 +222,7 @@
// Class to encapsulate the logic to decide when we are moving and which samples
// are safe to use.
-template <typename Position>
+template <typename Position, typename PositionBuffer>
class MoveDetector {
public:
MoveDetector(size_t filter_size) {
@@ -235,9 +240,10 @@
// buffer_size is the number of samples in the moving buffer, and
// zeroing_threshold is the max amount we can move within the period specified
// by buffer_size.
- bool Update(const Position &position, size_t buffer_size,
+ bool Update(const PositionBuffer &position_buffer, size_t buffer_size,
double zeroing_threshold) {
bool moving = true;
+ Position position(position_buffer);
if (buffered_samples_.size() < buffer_size) {
// Not enough samples to start determining if the robot is moving or not,
// don't use the samples yet.
@@ -312,9 +318,20 @@
}
// Returns information about our current state.
- State GetEstimatorState() const override;
+ virtual flatbuffers::Offset<State> GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const override;
private:
+ struct PositionStruct {
+ PositionStruct(const PotAndAbsolutePosition &position_buffer)
+ : absolute_encoder(position_buffer.absolute_encoder()),
+ encoder(position_buffer.encoder()),
+ pot(position_buffer.pot()) {}
+ double absolute_encoder;
+ double encoder;
+ double pot;
+ };
+
// The zeroing constants used to describe the configuration of the system.
const constants::PotAndAbsoluteEncoderZeroingConstants constants_;
@@ -335,7 +352,7 @@
// Offset between the Pot and Relative encoder position.
::std::vector<double> offset_samples_;
- MoveDetector<PotAndAbsolutePosition> move_detector_;
+ MoveDetector<PositionStruct, PotAndAbsolutePosition> move_detector_;
// Estimated offset between the pot and relative encoder.
double pot_relative_encoder_offset_ = 0;
@@ -380,7 +397,8 @@
void UpdateEstimate(const IndexPosition &info) override;
// Returns information about our current state.
- State GetEstimatorState() const override;
+ virtual flatbuffers::Offset<State> GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const override;
void TriggerError() override { error_ = true; }
@@ -453,9 +471,18 @@
}
// Returns information about our current state.
- State GetEstimatorState() const override;
+ virtual flatbuffers::Offset<State> GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const override;
private:
+ struct PositionStruct {
+ PositionStruct(const AbsolutePosition &position_buffer)
+ : absolute_encoder(position_buffer.absolute_encoder()),
+ encoder(position_buffer.encoder()) {}
+ double absolute_encoder;
+ double encoder;
+ };
+
// The zeroing constants used to describe the configuration of the system.
const constants::AbsoluteEncoderZeroingConstants constants_;
@@ -474,7 +501,7 @@
// absolute encoder.
::std::vector<double> relative_to_absolute_offset_samples_;
- MoveDetector<AbsolutePosition> move_detector_;
+ MoveDetector<PositionStruct, AbsolutePosition> move_detector_;
// Estimated start position of the mechanism
double offset_ = 0;
diff --git a/frc971/zeroing/zeroing_test.cc b/frc971/zeroing/zeroing_test.cc
index cca0585..61d2d0c 100644
--- a/frc971/zeroing/zeroing_test.cc
+++ b/frc971/zeroing/zeroing_test.cc
@@ -5,21 +5,20 @@
#include <random>
#include "gtest/gtest.h"
+#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/zeroing/zeroing.h"
-#include "frc971/control_loops/control_loops.q.h"
-#include "aos/testing/test_shm.h"
-#include "aos/util/thread.h"
#include "aos/die.h"
#include "frc971/control_loops/position_sensor_sim.h"
namespace frc971 {
namespace zeroing {
-using control_loops::PositionSensorSimulator;
using constants::AbsoluteEncoderZeroingConstants;
using constants::EncoderPlusIndexZeroingConstants;
using constants::PotAndAbsoluteEncoderZeroingConstants;
using constants::PotAndIndexPulseZeroingConstants;
+using control_loops::PositionSensorSimulator;
+using FBB = flatbuffers::FlatBufferBuilder;
static const size_t kSampleSize = 30;
static const double kAcceptableUnzeroedError = 0.2;
@@ -28,53 +27,58 @@
class ZeroingTest : public ::testing::Test {
protected:
- void SetUp() override { aos::SetDieTestMode(true); }
+ void SetUp() override {}
void MoveTo(PositionSensorSimulator *simulator,
PotAndIndexPulseZeroingEstimator *estimator,
double new_position) {
- PotAndIndexPosition sensor_values;
simulator->MoveTo(new_position);
- simulator->GetSensorValues(&sensor_values);
- estimator->UpdateEstimate(sensor_values);
+ FBB fbb;
+ estimator->UpdateEstimate(
+ *simulator->FillSensorValues<PotAndIndexPosition>(&fbb));
}
void MoveTo(PositionSensorSimulator *simulator,
- AbsoluteEncoderZeroingEstimator *estimator,
- double new_position) {
- AbsolutePosition sensor_values_;
+ AbsoluteEncoderZeroingEstimator *estimator, double new_position) {
simulator->MoveTo(new_position);
- simulator->GetSensorValues(&sensor_values_);
- estimator->UpdateEstimate(sensor_values_);
+ FBB fbb;
+ estimator->UpdateEstimate(
+ *simulator->FillSensorValues<AbsolutePosition>(&fbb));
}
void MoveTo(PositionSensorSimulator *simulator,
PotAndAbsoluteEncoderZeroingEstimator *estimator,
double new_position) {
- PotAndAbsolutePosition sensor_values_;
simulator->MoveTo(new_position);
- simulator->GetSensorValues(&sensor_values_);
- estimator->UpdateEstimate(sensor_values_);
+ FBB fbb;
+ estimator->UpdateEstimate(
+ *simulator->FillSensorValues<PotAndAbsolutePosition>(&fbb));
}
void MoveTo(PositionSensorSimulator *simulator,
PulseIndexZeroingEstimator *estimator, double new_position) {
- IndexPosition sensor_values_;
simulator->MoveTo(new_position);
- simulator->GetSensorValues(&sensor_values_);
- estimator->UpdateEstimate(sensor_values_);
+ FBB fbb;
+ estimator->UpdateEstimate(
+ *simulator->FillSensorValues<IndexPosition>(&fbb));
}
void MoveTo(PositionSensorSimulator *simulator,
HallEffectAndPositionZeroingEstimator *estimator,
double new_position) {
- HallEffectAndPosition sensor_values_;
simulator->MoveTo(new_position);
- simulator->GetSensorValues(&sensor_values_);
- estimator->UpdateEstimate(sensor_values_);
+ FBB fbb;
+ estimator->UpdateEstimate(
+ *simulator->FillSensorValues<HallEffectAndPosition>(&fbb));
}
- ::aos::testing::TestSharedMemory my_shm_;
+ template <typename T>
+ double GetEstimatorPosition(T *estimator) {
+ FBB fbb;
+ fbb.Finish(estimator->GetEstimatorState(&fbb));
+ return flatbuffers::GetRoot<typename T::State>(fbb.GetBufferPointer())
+ ->position();
+ }
};
TEST_F(ZeroingTest, TestMovingAverageFilter) {
@@ -90,13 +94,13 @@
for (int i = 0; i < 300; i++) {
MoveTo(&sim, &estimator, 3.3 * index_diff);
}
- ASSERT_NEAR(3.3 * index_diff, estimator.GetEstimatorState().position,
+ ASSERT_NEAR(3.3 * index_diff, GetEstimatorPosition(&estimator),
kAcceptableUnzeroedError * index_diff);
for (int i = 0; i < 300; i++) {
MoveTo(&sim, &estimator, 3.9 * index_diff);
}
- ASSERT_NEAR(3.9 * index_diff, estimator.GetEstimatorState().position,
+ ASSERT_NEAR(3.9 * index_diff, GetEstimatorPosition(&estimator),
kAcceptableUnzeroedError * index_diff);
}
@@ -133,25 +137,25 @@
for (int i = 0; i < 300; i++) {
MoveTo(&sim, &estimator, 3.6);
}
- ASSERT_NEAR(3.6, estimator.GetEstimatorState().position,
+ ASSERT_NEAR(3.6, GetEstimatorPosition(&estimator),
kAcceptableUnzeroedError * index_diff);
// With a single index pulse the zeroing estimator should be able to lock
// onto the true value of the position.
MoveTo(&sim, &estimator, 4.01);
- ASSERT_NEAR(4.01, estimator.GetEstimatorState().position, 0.001);
+ ASSERT_NEAR(4.01, GetEstimatorPosition(&estimator), 0.001);
MoveTo(&sim, &estimator, 4.99);
- ASSERT_NEAR(4.99, estimator.GetEstimatorState().position, 0.001);
+ ASSERT_NEAR(4.99, GetEstimatorPosition(&estimator), 0.001);
MoveTo(&sim, &estimator, 3.99);
- ASSERT_NEAR(3.99, estimator.GetEstimatorState().position, 0.001);
+ ASSERT_NEAR(3.99, GetEstimatorPosition(&estimator), 0.001);
MoveTo(&sim, &estimator, 3.01);
- ASSERT_NEAR(3.01, estimator.GetEstimatorState().position, 0.001);
+ ASSERT_NEAR(3.01, GetEstimatorPosition(&estimator), 0.001);
MoveTo(&sim, &estimator, 13.55);
- ASSERT_NEAR(13.55, estimator.GetEstimatorState().position, 0.001);
+ ASSERT_NEAR(13.55, GetEstimatorPosition(&estimator), 0.001);
}
TEST_F(ZeroingTest, TestDifferentIndexDiffs) {
@@ -167,25 +171,25 @@
for (int i = 0; i < 300; i++) {
MoveTo(&sim, &estimator, 3.5 * index_diff);
}
- ASSERT_NEAR(3.5 * index_diff, estimator.GetEstimatorState().position,
+ ASSERT_NEAR(3.5 * index_diff, GetEstimatorPosition(&estimator),
kAcceptableUnzeroedError * index_diff);
// With a single index pulse the zeroing estimator should be able to lock
// onto the true value of the position.
MoveTo(&sim, &estimator, 4.01);
- ASSERT_NEAR(4.01, estimator.GetEstimatorState().position, 0.001);
+ ASSERT_NEAR(4.01, GetEstimatorPosition(&estimator), 0.001);
MoveTo(&sim, &estimator, 4.99);
- ASSERT_NEAR(4.99, estimator.GetEstimatorState().position, 0.001);
+ ASSERT_NEAR(4.99, GetEstimatorPosition(&estimator), 0.001);
MoveTo(&sim, &estimator, 3.99);
- ASSERT_NEAR(3.99, estimator.GetEstimatorState().position, 0.001);
+ ASSERT_NEAR(3.99, GetEstimatorPosition(&estimator), 0.001);
MoveTo(&sim, &estimator, 3.01);
- ASSERT_NEAR(3.01, estimator.GetEstimatorState().position, 0.001);
+ ASSERT_NEAR(3.01, GetEstimatorPosition(&estimator), 0.001);
MoveTo(&sim, &estimator, 13.55);
- ASSERT_NEAR(13.55, estimator.GetEstimatorState().position, 0.001);
+ ASSERT_NEAR(13.55, GetEstimatorPosition(&estimator), 0.001);
}
TEST_F(ZeroingTest, TestPercentage) {
@@ -278,12 +282,12 @@
MoveTo(&sim, &estimator, 3.7 * index_diff);
ASSERT_TRUE(estimator.zeroed());
ASSERT_DOUBLE_EQ(3.3 * index_diff, estimator.offset());
- ASSERT_DOUBLE_EQ(3.7 * index_diff, estimator.GetEstimatorState().position);
+ ASSERT_DOUBLE_EQ(3.7 * index_diff, GetEstimatorPosition(&estimator));
// Trigger one more index pulse and check the offset.
MoveTo(&sim, &estimator, 4.7 * index_diff);
ASSERT_DOUBLE_EQ(3.3 * index_diff, estimator.offset());
- ASSERT_DOUBLE_EQ(4.7 * index_diff, estimator.GetEstimatorState().position);
+ ASSERT_DOUBLE_EQ(4.7 * index_diff, GetEstimatorPosition(&estimator));
}
TEST_F(ZeroingTest, BasicErrorAPITest) {
@@ -379,12 +383,12 @@
PotAndAbsoluteEncoderZeroingEstimator estimator(constants);
// We tolerate a couple NANs before we start.
- PotAndAbsolutePosition sensor_values;
- sensor_values.absolute_encoder = ::std::numeric_limits<double>::quiet_NaN();
- sensor_values.encoder = 0.0;
- sensor_values.pot = 0.0;
+ FBB fbb;
+ fbb.Finish(CreatePotAndAbsolutePosition(
+ fbb, 0.0, ::std::numeric_limits<double>::quiet_NaN(), 0.0));
for (size_t i = 0; i < kSampleSize - 1; ++i) {
- estimator.UpdateEstimate(sensor_values);
+ estimator.UpdateEstimate(
+ *flatbuffers::GetRoot<PotAndAbsolutePosition>(fbb.GetBufferPointer()));
}
for (size_t i = 0; i < kSampleSize + kMovingBufferSize - 1; ++i) {
@@ -431,17 +435,17 @@
PotAndAbsoluteEncoderZeroingEstimator estimator(constants);
- PotAndAbsolutePosition sensor_values;
- sensor_values.absolute_encoder = ::std::numeric_limits<double>::quiet_NaN();
- sensor_values.encoder = 0.0;
- sensor_values.pot = 0.0;
- // We tolerate a couple NANs before we start.
+ FBB fbb;
+ fbb.Finish(CreatePotAndAbsolutePosition(
+ fbb, 0.0, ::std::numeric_limits<double>::quiet_NaN(), 0.0));
+ const auto sensor_values =
+ flatbuffers::GetRoot<PotAndAbsolutePosition>(fbb.GetBufferPointer());
for (size_t i = 0; i < kSampleSize - 1; ++i) {
- estimator.UpdateEstimate(sensor_values);
+ estimator.UpdateEstimate(*sensor_values);
}
ASSERT_FALSE(estimator.error());
- estimator.UpdateEstimate(sensor_values);
+ estimator.UpdateEstimate(*sensor_values);
ASSERT_TRUE(estimator.error());
}
@@ -493,11 +497,11 @@
ASSERT_DOUBLE_EQ(start_pos, estimator.offset());
ASSERT_DOUBLE_EQ(3.5 * constants.index_difference,
- estimator.GetEstimatorState().position);
+ GetEstimatorPosition(&estimator));
MoveTo(&sim, &estimator, 0.5 * constants.index_difference);
ASSERT_DOUBLE_EQ(0.5 * constants.index_difference,
- estimator.GetEstimatorState().position);
+ GetEstimatorPosition(&estimator));
}
// Tests that we can detect when an index pulse occurs where we didn't expect
@@ -716,11 +720,13 @@
AbsoluteEncoderZeroingEstimator estimator(constants);
// We tolerate a couple NANs before we start.
- AbsolutePosition sensor_values;
- sensor_values.absolute_encoder = ::std::numeric_limits<double>::quiet_NaN();
- sensor_values.encoder = 0.0;
+ FBB fbb;
+ fbb.Finish(CreateAbsolutePosition(
+ fbb, 0.0, ::std::numeric_limits<double>::quiet_NaN()));
+ const auto sensor_values =
+ flatbuffers::GetRoot<AbsolutePosition>(fbb.GetBufferPointer());
for (size_t i = 0; i < kSampleSize - 1; ++i) {
- estimator.UpdateEstimate(sensor_values);
+ estimator.UpdateEstimate(*sensor_values);
}
for (size_t i = 0; i < kSampleSize + kMovingBufferSize - 1; ++i) {
@@ -769,16 +775,17 @@
AbsoluteEncoderZeroingEstimator estimator(constants);
- AbsolutePosition sensor_values;
- sensor_values.absolute_encoder = ::std::numeric_limits<double>::quiet_NaN();
- sensor_values.encoder = 0.0;
- // We tolerate a couple NANs before we start.
+ FBB fbb;
+ fbb.Finish(CreateAbsolutePosition(
+ fbb, 0.0, ::std::numeric_limits<double>::quiet_NaN()));
+ const auto sensor_values =
+ flatbuffers::GetRoot<AbsolutePosition>(fbb.GetBufferPointer());
for (size_t i = 0; i < kSampleSize - 1; ++i) {
- estimator.UpdateEstimate(sensor_values);
+ estimator.UpdateEstimate(*sensor_values);
}
ASSERT_FALSE(estimator.error());
- estimator.UpdateEstimate(sensor_values);
+ estimator.UpdateEstimate(*sensor_values);
ASSERT_TRUE(estimator.error());
}