Convert aos over to flatbuffers
Everything builds, and all the tests pass. I suspect that some entries
are missing from the config files, but those will be found pretty
quickly on startup.
There is no logging or live introspection of queue messages.
Change-Id: I496ee01ed68f202c7851bed7e8786cee30df29f5
diff --git a/y2019/actors/autonomous_actor.cc b/y2019/actors/autonomous_actor.cc
index a5a71aa..fbde0be 100644
--- a/y2019/actors/autonomous_actor.cc
+++ b/y2019/actors/autonomous_actor.cc
@@ -8,14 +8,16 @@
#include "aos/logging/logging.h"
#include "aos/util/phased_loop.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/control_loops/drivetrain/localizer.q.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "y2019/actors/auto_splines.h"
#include "y2019/control_loops/drivetrain/drivetrain_base.h"
namespace y2019 {
namespace actors {
+
+using ::frc971::ProfileParametersT;
using ::aos::monotonic_clock;
+using frc971::control_loops::drivetrain::LocalizerControl;
namespace chrono = ::std::chrono;
AutonomousActor::AutonomousActor(::aos::EventLoop *event_loop)
@@ -24,15 +26,14 @@
localizer_control_sender_(
event_loop->MakeSender<
::frc971::control_loops::drivetrain::LocalizerControl>(
- ".frc971.control_loops.drivetrain.localizer_control")),
+ "/drivetrain")),
superstructure_goal_sender_(
- event_loop->MakeSender<::y2019::control_loops::superstructure::
- SuperstructureQueue::Goal>(
- ".y2019.control_loops.superstructure.superstructure_queue.goal")),
- superstructure_status_fetcher_(event_loop->MakeFetcher<
- ::y2019::control_loops::superstructure::
- SuperstructureQueue::Status>(
- ".y2019.control_loops.superstructure.superstructure_queue.status")) {}
+ event_loop->MakeSender<::y2019::control_loops::superstructure::Goal>(
+ "/superstructure")),
+ superstructure_status_fetcher_(
+ event_loop
+ ->MakeFetcher<::y2019::control_loops::superstructure::Status>(
+ "/superstructure")) {}
bool AutonomousActor::WaitForDriveXGreater(double x) {
AOS_LOG(INFO, "Waiting until x > %f\n", x);
@@ -46,8 +47,8 @@
}
phased_loop.SleepUntilNext();
drivetrain_status_fetcher_.Fetch();
- if (drivetrain_status_fetcher_->x > x) {
- AOS_LOG(INFO, "X at %f\n", drivetrain_status_fetcher_->x);
+ if (drivetrain_status_fetcher_->x() > x) {
+ AOS_LOG(INFO, "X at %f\n", drivetrain_status_fetcher_->x());
return true;
}
}
@@ -65,8 +66,8 @@
}
phased_loop.SleepUntilNext();
drivetrain_status_fetcher_.Fetch();
- if (::std::abs(drivetrain_status_fetcher_->y) < y) {
- AOS_LOG(INFO, "Y at %f\n", drivetrain_status_fetcher_->y);
+ if (::std::abs(drivetrain_status_fetcher_->y()) < y) {
+ AOS_LOG(INFO, "Y at %f\n", drivetrain_status_fetcher_->y());
return true;
}
}
@@ -90,13 +91,16 @@
SendSuperstructureGoal();
{
- auto localizer_resetter = localizer_control_sender_.MakeMessage();
+ auto builder = localizer_control_sender_.MakeBuilder();
+
+ LocalizerControl::Builder localizer_control_builder =
+ builder.MakeBuilder<LocalizerControl>();
// Start on the left l2.
- localizer_resetter->x = 1.0;
- localizer_resetter->y = 1.35 * turn_scalar;
- localizer_resetter->theta = M_PI;
- localizer_resetter->theta_uncertainty = 0.00001;
- if (!localizer_resetter.Send()) {
+ localizer_control_builder.add_x(1.0);
+ localizer_control_builder.add_y(1.35 * turn_scalar);
+ localizer_control_builder.add_theta(M_PI);
+ localizer_control_builder.add_theta_uncertainty(0.00001);
+ if (!builder.Send(localizer_control_builder.Finish())) {
AOS_LOG(ERROR, "Failed to reset localizer.\n");
}
}
@@ -105,20 +109,28 @@
// Otherwise our drivetrain reset will do a 180 right at the start.
WaitUntil([this]() { return drivetrain_status_fetcher_.Fetch(); });
AOS_LOG(INFO, "Heading is %f\n",
- drivetrain_status_fetcher_->estimated_heading);
+ drivetrain_status_fetcher_->estimated_heading());
InitializeEncoders();
ResetDrivetrain();
WaitUntil([this]() { return drivetrain_status_fetcher_.Fetch(); });
AOS_LOG(INFO, "Heading is %f\n",
- drivetrain_status_fetcher_->estimated_heading);
+ drivetrain_status_fetcher_->estimated_heading());
ResetDrivetrain();
InitializeEncoders();
}
-const ProfileParameters kJumpDrive = {2.0, 3.0};
-const ProfileParameters kDrive = {4.0, 3.0};
-const ProfileParameters kTurn = {5.0, 15.0};
+ProfileParametersT MakeProfileParameters(float max_velocity,
+ float max_acceleration) {
+ ProfileParametersT result;
+ result.max_velocity = max_velocity;
+ result.max_acceleration = max_acceleration;
+ return result;
+}
+
+const ProfileParametersT kJumpDrive = MakeProfileParameters(2.0, 3.0);
+const ProfileParametersT kDrive = MakeProfileParameters(4.0, 3.0);
+const ProfileParametersT kTurn = MakeProfileParameters(5.0, 15.0);
const ElevatorWristPosition kPanelHPIntakeForwrdPos{0.01, M_PI / 2.0};
const ElevatorWristPosition kPanelHPIntakeBackwardPos{0.015, -M_PI / 2.0};
@@ -130,14 +142,23 @@
const ElevatorWristPosition kPanelCargoBackwardPos{0.0, -M_PI / 2.0};
+template <typename Functor>
+std::function<flatbuffers::Offset<frc971::MultiSpline>(
+ aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder)>
+BindIsLeft(Functor f, bool is_left) {
+ return
+ [is_left, f](aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder
+ *builder) { return f(builder, is_left); };
+}
+
bool AutonomousActor::RunAction(
- const ::frc971::autonomous::AutonomousActionParams ¶ms) {
+ const ::frc971::autonomous::AutonomousActionParams *params) {
const monotonic_clock::time_point start_time = monotonic_now();
- const bool is_left = params.mode == 0;
+ const bool is_left = params->mode() == 0;
{
AOS_LOG(INFO, "Starting autonomous action with mode %" PRId32 " %s\n",
- params.mode, is_left ? "left" : "right");
+ params->mode(), is_left ? "left" : "right");
}
const double turn_scalar = is_left ? 1.0 : -1.0;
@@ -147,7 +168,7 @@
Mode mode = Mode::kCargoship;
if (mode == Mode::kRocket) {
SplineHandle spline1 =
- PlanSpline(AutonomousSplines::HabToFarRocketTest(is_left),
+ PlanSpline(BindIsLeft(AutonomousSplines::HabToFarRocketTest, is_left),
SplineDirection::kBackward);
// Grab the disk, jump, wait until we have vacuum, then raise the elevator
@@ -176,15 +197,18 @@
// END SPLINE 1
if (!spline1.WaitForSplineDistanceRemaining(0.2)) return true;
- LineFollowAtVelocity(1.3, 4);
+ LineFollowAtVelocity(1.3,
+ control_loops::drivetrain::SelectionHint_FAR_ROCKET);
if (!WaitForMilliseconds(::std::chrono::milliseconds(1200))) return true;
set_suction_goal(false, 1);
SendSuperstructureGoal();
if (!WaitForMilliseconds(::std::chrono::milliseconds(200))) return true;
- LineFollowAtVelocity(-1.0, 4);
- SplineHandle spline2 = PlanSpline(AutonomousSplines::FarRocketToHP(is_left),
- SplineDirection::kBackward);
+ LineFollowAtVelocity(-1.0,
+ control_loops::drivetrain::SelectionHint_FAR_ROCKET);
+ SplineHandle spline2 =
+ PlanSpline(BindIsLeft(AutonomousSplines::FarRocketToHP, is_left),
+ SplineDirection::kBackward);
if (!WaitForMilliseconds(::std::chrono::milliseconds(150))) return true;
if (!spline2.WaitForPlan()) return true;
@@ -204,8 +228,9 @@
// As soon as we pick up Panel 2 go score on the back rocket
if (!WaitForGamePiece()) return true;
LineFollowAtVelocity(1.5);
- SplineHandle spline3 = PlanSpline(AutonomousSplines::HPToFarRocket(is_left),
- SplineDirection::kForward);
+ SplineHandle spline3 =
+ PlanSpline(BindIsLeft(AutonomousSplines::HPToFarRocket, is_left),
+ SplineDirection::kForward);
if (!WaitForDriveXGreater(0.50)) return true;
if (!spline3.WaitForPlan()) return true;
spline3.Start();
@@ -214,7 +239,8 @@
set_elevator_wrist_goal(kPanelBackwardMiddlePos);
SendSuperstructureGoal();
if (!WaitForDriveXGreater(7.1)) return true;
- LineFollowAtVelocity(-1.5, 4);
+ LineFollowAtVelocity(-1.5,
+ control_loops::drivetrain::SelectionHint_FAR_ROCKET);
if (!WaitForMilliseconds(::std::chrono::milliseconds(1000))) return true;
set_elevator_wrist_goal(kPanelBackwardUpperPos);
SendSuperstructureGoal();
@@ -222,13 +248,14 @@
set_suction_goal(false, 1);
SendSuperstructureGoal();
if (!WaitForMilliseconds(::std::chrono::milliseconds(400))) return true;
- LineFollowAtVelocity(1.0, 4);
+ LineFollowAtVelocity(1.0,
+ control_loops::drivetrain::SelectionHint_FAR_ROCKET);
SendSuperstructureGoal();
if (!WaitForMilliseconds(::std::chrono::milliseconds(200))) return true;
} else if (mode == Mode::kCargoship) {
- SplineHandle spline1 =
- PlanSpline(AutonomousSplines::HABToSecondCargoShipBay(is_left),
- SplineDirection::kBackward);
+ SplineHandle spline1 = PlanSpline(
+ BindIsLeft(AutonomousSplines::HABToSecondCargoShipBay, is_left),
+ SplineDirection::kBackward);
set_elevator_goal(0.01);
set_wrist_goal(-M_PI / 2.0);
set_intake_goal(-1.2);
@@ -254,7 +281,8 @@
if (!spline1.WaitForSplineDistanceRemaining(0.8)) return true;
// Line follow in to the first disc.
- LineFollowAtVelocity(-0.9, 2);
+ LineFollowAtVelocity(-0.9,
+ control_loops::drivetrain::SelectionHint_MID_SHIP);
if (!WaitForDriveYCloseToZero(1.2)) return true;
set_suction_goal(false, 1);
@@ -265,10 +293,11 @@
if (!WaitForDriveYCloseToZero(1.13)) return true;
if (!WaitForMilliseconds(::std::chrono::milliseconds(300))) return true;
- LineFollowAtVelocity(0.9, 2);
- SplineHandle spline2 =
- PlanSpline(AutonomousSplines::SecondCargoShipBayToHP(is_left),
- SplineDirection::kForward);
+ LineFollowAtVelocity(0.9,
+ control_loops::drivetrain::SelectionHint_MID_SHIP);
+ SplineHandle spline2 = PlanSpline(
+ BindIsLeft(AutonomousSplines::SecondCargoShipBayToHP, is_left),
+ SplineDirection::kForward);
if (!WaitForMilliseconds(::std::chrono::milliseconds(400))) return true;
if (!spline2.WaitForPlan()) return true;
AOS_LOG(INFO, "Planned\n");
@@ -288,9 +317,9 @@
AOS_LOG(INFO, "Got gamepiece %f\n",
::aos::time::DurationInSeconds(monotonic_now() - start_time));
LineFollowAtVelocity(-4.0);
- SplineHandle spline3 =
- PlanSpline(AutonomousSplines::HPToThirdCargoShipBay(is_left),
- SplineDirection::kBackward);
+ SplineHandle spline3 = PlanSpline(
+ BindIsLeft(AutonomousSplines::HPToThirdCargoShipBay, is_left),
+ SplineDirection::kBackward);
if (!WaitForDriveXGreater(0.55)) return true;
if (!spline3.WaitForPlan()) return true;
spline3.Start();
@@ -301,7 +330,8 @@
if (!spline3.WaitForSplineDistanceRemaining(0.7)) return true;
// Line follow in to the second disc.
- LineFollowAtVelocity(-0.7, 3);
+ LineFollowAtVelocity(-0.7,
+ control_loops::drivetrain::SelectionHint_FAR_SHIP);
AOS_LOG(INFO, "Drawing in disc 2 %f\n",
::aos::time::DurationInSeconds(monotonic_now() - start_time));
if (!WaitForDriveYCloseToZero(1.2)) return true;
@@ -315,7 +345,8 @@
if (!WaitForMilliseconds(::std::chrono::milliseconds(200))) return true;
AOS_LOG(INFO, "Backing up %f\n",
::aos::time::DurationInSeconds(monotonic_now() - start_time));
- LineFollowAtVelocity(0.9, 3);
+ LineFollowAtVelocity(0.9,
+ control_loops::drivetrain::SelectionHint_FAR_SHIP);
if (!WaitForMilliseconds(::std::chrono::milliseconds(400))) return true;
} else {
// Grab the disk, wait until we have vacuum, then jump