Convert aos over to flatbuffers
Everything builds, and all the tests pass. I suspect that some entries
are missing from the config files, but those will be found pretty
quickly on startup.
There is no logging or live introspection of queue messages.
Change-Id: I496ee01ed68f202c7851bed7e8786cee30df29f5
diff --git a/y2018/control_loops/superstructure/arm/BUILD b/y2018/control_loops/superstructure/arm/BUILD
index e8af2b9..bf3c306 100644
--- a/y2018/control_loops/superstructure/arm/BUILD
+++ b/y2018/control_loops/superstructure/arm/BUILD
@@ -12,7 +12,7 @@
"//aos/logging",
"//frc971/control_loops:dlqr",
"//frc971/control_loops:jacobian",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -27,7 +27,7 @@
":ekf",
":trajectory",
"//aos/testing:googletest",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -42,8 +42,8 @@
visibility = ["//visibility:public"],
deps = [
"//frc971/control_loops:runge_kutta",
- "//third_party/eigen",
"@com_github_gflags_gflags//:gflags",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -77,9 +77,9 @@
":ekf",
":generated_graph",
":trajectory",
- "//third_party/eigen",
"//third_party/matplotlib-cpp",
"@com_github_gflags_gflags//:gflags",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -95,7 +95,7 @@
deps = [
":dynamics",
"//frc971/control_loops:jacobian",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -129,10 +129,10 @@
":generated_graph",
":graph",
":trajectory",
- "//aos/logging:queue_logging",
"//frc971/zeroing",
"//y2018:constants",
- "//y2018/control_loops/superstructure:superstructure_queue",
+ "//y2018/control_loops/superstructure:superstructure_position_fbs",
+ "//y2018/control_loops/superstructure:superstructure_status_fbs",
],
)
diff --git a/y2018/control_loops/superstructure/arm/arm.cc b/y2018/control_loops/superstructure/arm/arm.cc
index 2ae1996..1f904e2 100644
--- a/y2018/control_loops/superstructure/arm/arm.cc
+++ b/y2018/control_loops/superstructure/arm/arm.cc
@@ -4,7 +4,6 @@
#include <iostream>
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
#include "y2018/constants.h"
#include "y2018/control_loops/superstructure/arm/demo_path.h"
#include "y2018/control_loops/superstructure/arm/dynamics.h"
@@ -44,15 +43,15 @@
void Arm::Reset() { state_ = State::UNINITIALIZED; }
-void Arm::Iterate(const ::aos::monotonic_clock::time_point monotonic_now,
- const uint32_t *unsafe_goal, bool grab_box, bool open_claw,
- bool close_claw, const control_loops::ArmPosition *position,
- const bool claw_beambreak_triggered,
- const bool box_back_beambreak_triggered,
- const bool intake_clear_of_box, bool suicide,
- bool trajectory_override, double *proximal_output,
- double *distal_output, bool *release_arm_brake,
- bool *claw_closed, control_loops::ArmStatus *status) {
+flatbuffers::Offset<superstructure::ArmStatus> Arm::Iterate(
+ const ::aos::monotonic_clock::time_point monotonic_now,
+ const uint32_t *unsafe_goal, bool grab_box, bool open_claw, bool close_claw,
+ const superstructure::ArmPosition *position,
+ const bool claw_beambreak_triggered,
+ const bool box_back_beambreak_triggered, const bool intake_clear_of_box,
+ bool suicide, bool trajectory_override, double *proximal_output,
+ double *distal_output, bool *release_arm_brake, bool *claw_closed,
+ flatbuffers::FlatBufferBuilder *fbb) {
::Eigen::Matrix<double, 2, 1> Y;
const bool outputs_disabled =
((proximal_output == nullptr) || (distal_output == nullptr) ||
@@ -86,11 +85,11 @@
claw_closed_count_ = 50;
}
- Y << position->proximal.encoder + proximal_offset_,
- position->distal.encoder + distal_offset_;
+ Y << position->proximal()->encoder() + proximal_offset_,
+ position->distal()->encoder() + distal_offset_;
- proximal_zeroing_estimator_.UpdateEstimate(position->proximal);
- distal_zeroing_estimator_.UpdateEstimate(position->distal);
+ proximal_zeroing_estimator_.UpdateEstimate(*position->proximal());
+ distal_zeroing_estimator_.UpdateEstimate(*position->distal());
if (proximal_output != nullptr) {
*proximal_output = 0.0;
@@ -128,8 +127,8 @@
proximal_offset_ = proximal_zeroing_estimator_.offset();
distal_offset_ = distal_zeroing_estimator_.offset();
- Y << position->proximal.encoder + proximal_offset_,
- position->distal.encoder + distal_offset_;
+ Y << position->proximal()->encoder() + proximal_offset_,
+ position->distal()->encoder() + distal_offset_;
// TODO(austin): Offset ekf rather than reset it. Since we aren't
// moving at this point, it's pretty safe to do this.
@@ -377,17 +376,29 @@
follower_.Update(arm_ekf_.X_hat(), disable, kDt(), vmax_,
max_operating_voltage);
AOS_LOG(INFO, "Max voltage: %f\n", max_operating_voltage);
- status->goal_theta0 = follower_.theta(0);
- status->goal_theta1 = follower_.theta(1);
- status->goal_omega0 = follower_.omega(0);
- status->goal_omega1 = follower_.omega(1);
- status->theta0 = arm_ekf_.X_hat(0);
- status->theta1 = arm_ekf_.X_hat(2);
- status->omega0 = arm_ekf_.X_hat(1);
- status->omega1 = arm_ekf_.X_hat(3);
- status->voltage_error0 = arm_ekf_.X_hat(4);
- status->voltage_error1 = arm_ekf_.X_hat(5);
+ flatbuffers::Offset<frc971::PotAndAbsoluteEncoderEstimatorState>
+ proximal_estimator_state_offset =
+ proximal_zeroing_estimator_.GetEstimatorState(fbb);
+ flatbuffers::Offset<frc971::PotAndAbsoluteEncoderEstimatorState>
+ distal_estimator_state_offset =
+ distal_zeroing_estimator_.GetEstimatorState(fbb);
+
+ superstructure::ArmStatus::Builder status_builder(*fbb);
+ status_builder.add_proximal_estimator_state(proximal_estimator_state_offset);
+ status_builder.add_distal_estimator_state(distal_estimator_state_offset);
+
+ status_builder.add_goal_theta0(follower_.theta(0));
+ status_builder.add_goal_theta1(follower_.theta(1));
+ status_builder.add_goal_omega0(follower_.omega(0));
+ status_builder.add_goal_omega1(follower_.omega(1));
+
+ status_builder.add_theta0(arm_ekf_.X_hat(0));
+ status_builder.add_theta1(arm_ekf_.X_hat(2));
+ status_builder.add_omega0(arm_ekf_.X_hat(1));
+ status_builder.add_omega1(arm_ekf_.X_hat(3));
+ status_builder.add_voltage_error0(arm_ekf_.X_hat(4));
+ status_builder.add_voltage_error1(arm_ekf_.X_hat(5));
if (!disable) {
*proximal_output = ::std::max(
@@ -402,22 +413,17 @@
*claw_closed = claw_closed_;
}
- status->proximal_estimator_state =
- proximal_zeroing_estimator_.GetEstimatorState();
- status->distal_estimator_state =
- distal_zeroing_estimator_.GetEstimatorState();
+ status_builder.add_path_distance_to_go(follower_.path_distance_to_go());
+ status_builder.add_current_node(current_node_);
- status->path_distance_to_go = follower_.path_distance_to_go();
- status->current_node = current_node_;
-
- status->zeroed = (proximal_zeroing_estimator_.zeroed() &&
- distal_zeroing_estimator_.zeroed());
- status->estopped = (state_ == State::ESTOP);
- status->state = static_cast<int32_t>(state_);
- status->grab_state = static_cast<int32_t>(grab_state_);
- status->failed_solutions = follower_.failed_solutions();
+ status_builder.add_zeroed(zeroed());
+ status_builder.add_estopped(estopped());
+ status_builder.add_state(static_cast<int32_t>(state_));
+ status_builder.add_grab_state(static_cast<int32_t>(grab_state_));
+ status_builder.add_failed_solutions(follower_.failed_solutions());
arm_ekf_.Predict(follower_.U(), kDt());
+ return status_builder.Finish();
}
} // namespace arm
diff --git a/y2018/control_loops/superstructure/arm/arm.h b/y2018/control_loops/superstructure/arm/arm.h
index 8cd6b39..7ceb001 100644
--- a/y2018/control_loops/superstructure/arm/arm.h
+++ b/y2018/control_loops/superstructure/arm/arm.h
@@ -1,6 +1,7 @@
#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_ARM_H_
#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_ARM_H_
+#include "aos/time/time.h"
#include "frc971/zeroing/zeroing.h"
#include "y2018/constants.h"
#include "y2018/control_loops/superstructure/arm/dynamics.h"
@@ -8,7 +9,8 @@
#include "y2018/control_loops/superstructure/arm/generated_graph.h"
#include "y2018/control_loops/superstructure/arm/graph.h"
#include "y2018/control_loops/superstructure/arm/trajectory.h"
-#include "y2018/control_loops/superstructure/superstructure.q.h"
+#include "y2018/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2018/control_loops/superstructure/superstructure_status_generated.h"
namespace y2018 {
namespace control_loops {
@@ -34,15 +36,15 @@
static constexpr double kPathlessVMax() { return 5.0; }
static constexpr double kGotoPathVMax() { return 6.0; }
- void Iterate(const ::aos::monotonic_clock::time_point monotonic_now,
- const uint32_t *unsafe_goal, bool grab_box, bool open_claw,
- bool close_claw, const control_loops::ArmPosition *position,
- const bool claw_beambreak_triggered,
- const bool box_back_beambreak_triggered,
- const bool intake_clear_of_box, bool suicide,
- bool trajectory_override, double *proximal_output,
- double *distal_output, bool *release_arm_brake,
- bool *claw_closed, control_loops::ArmStatus *status);
+ flatbuffers::Offset<superstructure::ArmStatus> Iterate(
+ const ::aos::monotonic_clock::time_point monotonic_now,
+ const uint32_t *unsafe_goal, bool grab_box, bool open_claw,
+ bool close_claw, const superstructure::ArmPosition *position,
+ const bool claw_beambreak_triggered,
+ const bool box_back_beambreak_triggered, const bool intake_clear_of_box,
+ bool suicide, bool trajectory_override, double *proximal_output,
+ double *distal_output, bool *release_arm_brake, bool *claw_closed,
+ flatbuffers::FlatBufferBuilder *fbb);
void Reset();
@@ -68,6 +70,12 @@
State state() const { return state_; }
GrabState grab_state() const { return grab_state_; }
+ bool estopped() const { return state_ == State::ESTOP; }
+ bool zeroed() const {
+ return (proximal_zeroing_estimator_.zeroed() &&
+ distal_zeroing_estimator_.zeroed());
+ }
+
// Returns the maximum position for the intake. This is used to override the
// intake position to release the box when the state machine is lifting.
double max_intake_override() const { return max_intake_override_; }