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/y2017/control_loops/superstructure/column/BUILD b/y2017/control_loops/superstructure/column/BUILD
index 694dd47..63dd98c 100644
--- a/y2017/control_loops/superstructure/column/BUILD
+++ b/y2017/control_loops/superstructure/column/BUILD
@@ -1,88 +1,89 @@
genrule(
- name = 'genrule_column',
- cmd = '$(location //y2017/control_loops/python:column) $(OUTS)',
- tools = [
- '//y2017/control_loops/python:column',
- ],
- outs = [
- 'column_plant.h',
- 'column_plant.cc',
- 'column_integral_plant.h',
- 'column_integral_plant.cc',
- 'stuck_column_integral_plant.h',
- 'stuck_column_integral_plant.cc',
- ],
+ name = "genrule_column",
+ outs = [
+ "column_plant.h",
+ "column_plant.cc",
+ "column_integral_plant.h",
+ "column_integral_plant.cc",
+ "stuck_column_integral_plant.h",
+ "stuck_column_integral_plant.cc",
+ ],
+ cmd = "$(location //y2017/control_loops/python:column) $(OUTS)",
+ tools = [
+ "//y2017/control_loops/python:column",
+ ],
)
cc_library(
- name = 'column_plants',
- visibility = ['//visibility:public'],
- srcs = [
- 'column_plant.cc',
- 'column_integral_plant.cc',
- 'stuck_column_integral_plant.cc',
- ],
- hdrs = [
- 'column_plant.h',
- 'column_integral_plant.h',
- 'stuck_column_integral_plant.h',
- ],
- deps = [
- '//frc971/control_loops:state_feedback_loop',
- ],
+ name = "column_plants",
+ srcs = [
+ "column_integral_plant.cc",
+ "column_plant.cc",
+ "stuck_column_integral_plant.cc",
+ ],
+ hdrs = [
+ "column_integral_plant.h",
+ "column_plant.h",
+ "stuck_column_integral_plant.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:state_feedback_loop",
+ ],
)
cc_library(
- name = 'column',
- visibility = ['//visibility:public'],
- srcs = [
- 'column.cc',
- ],
- hdrs = [
- 'column.h',
- ],
- deps = [
- ':column_plants',
- ':column_zeroing',
- '//aos/controls:control_loop',
- '//aos:math',
- '//frc971/control_loops:profiled_subsystem',
- '//y2017/control_loops/superstructure/intake:intake',
- '//y2017/control_loops/superstructure:superstructure_queue',
- '//y2017/control_loops/superstructure:vision_time_adjuster',
- '//y2017:constants',
- ],
+ name = "column",
+ srcs = [
+ "column.cc",
+ ],
+ hdrs = [
+ "column.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":column_plants",
+ ":column_zeroing",
+ "//aos:math",
+ "//aos/controls:control_loop",
+ "//frc971/control_loops:profiled_subsystem",
+ "//y2017:constants",
+ "//y2017/control_loops/superstructure:superstructure_position_fbs",
+ "//y2017/control_loops/superstructure:superstructure_status_fbs",
+ "//y2017/control_loops/superstructure:vision_time_adjuster",
+ "//y2017/control_loops/superstructure/intake",
+ ],
)
cc_library(
- name = 'column_zeroing',
- srcs = [
- 'column_zeroing.cc',
- ],
- hdrs = [
- 'column_zeroing.h',
- ],
- deps = [
- '//frc971/control_loops:queues',
- '//frc971/zeroing:wrap',
- '//frc971/zeroing:zeroing',
- '//frc971:constants',
- '//y2017/control_loops/superstructure:superstructure_queue',
- '//y2017:constants',
- ],
+ name = "column_zeroing",
+ srcs = [
+ "column_zeroing.cc",
+ ],
+ hdrs = [
+ "column_zeroing.h",
+ ],
+ deps = [
+ "//frc971:constants",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//frc971/zeroing",
+ "//frc971/zeroing:wrap",
+ "//y2017:constants",
+ "//y2017/control_loops/superstructure:superstructure_position_fbs",
+ "//y2017/control_loops/superstructure:superstructure_status_fbs",
+ ],
)
cc_test(
- name = 'column_zeroing_test',
- srcs = [
- 'column_zeroing_test.cc',
- ],
- deps = [
- ':column_zeroing',
- '//aos/testing:test_shm',
- '//frc971/control_loops:position_sensor_sim',
- '//frc971/control_loops:team_number_test_environment',
- '//y2017/control_loops/superstructure:superstructure_queue',
- '//y2017:constants',
- ],
+ name = "column_zeroing_test",
+ srcs = [
+ "column_zeroing_test.cc",
+ ],
+ deps = [
+ ":column_zeroing",
+ "//aos/testing:test_shm",
+ "//frc971/control_loops:position_sensor_sim",
+ "//frc971/control_loops:team_number_test_environment",
+ "//y2017:constants",
+ ],
)
diff --git a/y2017/control_loops/superstructure/column/column.cc b/y2017/control_loops/superstructure/column/column.cc
index ab1ff72..b2e7c5c 100644
--- a/y2017/control_loops/superstructure/column/column.cc
+++ b/y2017/control_loops/superstructure/column/column.cc
@@ -118,17 +118,18 @@
}
turret_last_position_ = turret_position();
- Y_ << new_position.indexer.encoder, new_position.turret.encoder;
+ Y_ << new_position.indexer()->encoder(), new_position.turret()->encoder();
Y_ += offset_;
loop_->Correct(Y_);
- indexer_history_[indexer_history_position_] = new_position.indexer.encoder;
+ indexer_history_[indexer_history_position_] =
+ new_position.indexer()->encoder();
indexer_history_position_ = (indexer_history_position_ + 1) % kHistoryLength;
indexer_dt_velocity_ =
- (new_position.indexer.encoder - indexer_last_position_) /
+ (new_position.indexer()->encoder() - indexer_last_position_) /
::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency);
- indexer_last_position_ = new_position.indexer.encoder;
+ indexer_last_position_ = new_position.indexer()->encoder();
stuck_indexer_detector_->Correct(Y_);
@@ -304,9 +305,11 @@
}
void ColumnProfiledSubsystem::AdjustProfile(
- const ::frc971::ProfileParameters &profile_parameters) {
- AdjustProfile(profile_parameters.max_velocity,
- profile_parameters.max_acceleration);
+ const ::frc971::ProfileParameters *profile_parameters) {
+ AdjustProfile(
+ profile_parameters == nullptr ? 0.0 : profile_parameters->max_velocity(),
+ profile_parameters == nullptr ? 0.0
+ : profile_parameters->max_acceleration());
}
void ColumnProfiledSubsystem::AdjustProfile(double max_angular_velocity,
@@ -348,20 +351,46 @@
stuck_indexer_detector_->mutable_X_hat(5, 0) = 0.0;
}
-void ColumnProfiledSubsystem::PopulateIndexerStatus(IndexerStatus *status) {
- status->avg_angular_velocity = indexer_average_angular_velocity_;
+void ColumnProfiledSubsystem::PopulateIndexerStatus(
+ IndexerStatus::Builder *status_builder) {
+ status_builder->add_avg_angular_velocity(indexer_average_angular_velocity_);
- status->angular_velocity = X_hat_current_(1, 0);
- status->ready = indexer_ready_;
+ status_builder->add_angular_velocity(X_hat_current_(1, 0));
+ status_builder->add_ready(indexer_ready_);
- status->voltage_error = X_hat_current_(4, 0);
- status->stuck_voltage_error = stuck_indexer_X_hat_current_(4, 0);
- status->position_error = indexer_position_error_;
- status->instantaneous_velocity = indexer_dt_velocity_;
+ status_builder->add_voltage_error(X_hat_current_(4, 0));
+ status_builder->add_stuck_voltage_error(stuck_indexer_X_hat_current_(4, 0));
+ status_builder->add_position_error(indexer_position_error_);
+ status_builder->add_instantaneous_velocity(indexer_dt_velocity_);
- status->stuck = IsIndexerStuck();
+ status_builder->add_stuck(IsIndexerStuck());
- status->stuck_voltage = IndexerStuckVoltage();
+ status_builder->add_stuck_voltage(IndexerStuckVoltage());
+}
+
+void ColumnProfiledSubsystem::PopulateTurretStatus(
+ TurretProfiledSubsystemStatus::Builder *status_builder,
+ flatbuffers::Offset<ColumnZeroingEstimator::State> estimator_state_offset) {
+ status_builder->add_zeroed(zeroed());
+
+ status_builder->add_position(X_hat(2, 0));
+ status_builder->add_velocity(X_hat(3, 0));
+ status_builder->add_goal_position(goal(2, 0));
+ status_builder->add_goal_velocity(goal(3, 0));
+ status_builder->add_unprofiled_goal_position(unprofiled_goal(2, 0));
+ status_builder->add_unprofiled_goal_velocity(unprofiled_goal(3, 0));
+ status_builder->add_voltage_error(X_hat(5, 0));
+ status_builder->add_calculated_velocity(
+ (turret_position() - turret_last_position_) /
+ ::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency));
+
+ status_builder->add_estimator_state(estimator_state_offset);
+
+ Eigen::Matrix<double, 6, 1> error = controller().error();
+ status_builder->add_position_power(controller().controller().K(0, 0) *
+ error(0, 0));
+ status_builder->add_velocity_power(controller().controller().K(0, 1) *
+ error(1, 0));
}
Column::Column(::aos::EventLoop *event_loop)
@@ -384,15 +413,15 @@
freeze_ = false;
}
-void Column::Iterate(const ::aos::monotonic_clock::time_point monotonic_now,
- const control_loops::IndexerGoal *unsafe_indexer_goal,
- const control_loops::TurretGoal *unsafe_turret_goal,
- const ColumnPosition *position,
- const vision::VisionStatus *vision_status,
- double *indexer_output, double *turret_output,
- IndexerStatus *indexer_status,
- TurretProfiledSubsystemStatus *turret_status,
- intake::Intake *intake) {
+std::pair<flatbuffers::Offset<IndexerStatus>,
+ flatbuffers::Offset<TurretProfiledSubsystemStatus>>
+Column::Iterate(const ::aos::monotonic_clock::time_point monotonic_now,
+ const IndexerGoalT *unsafe_indexer_goal,
+ const TurretGoal *unsafe_turret_goal,
+ const ColumnPosition *position,
+ const vision::VisionStatus *vision_status,
+ double *indexer_output, double *turret_output,
+ flatbuffers::FlatBufferBuilder *fbb, intake::Intake *intake) {
bool disable = turret_output == nullptr || indexer_output == nullptr;
profiled_subsystem_.Correct(*position);
@@ -526,11 +555,12 @@
}
if (unsafe_turret_goal && unsafe_indexer_goal) {
- profiled_subsystem_.AdjustProfile(unsafe_turret_goal->profile_params);
+ profiled_subsystem_.AdjustProfile(unsafe_turret_goal->profile_params());
profiled_subsystem_.set_unprofiled_goal(
- unsafe_indexer_goal->angular_velocity, unsafe_turret_goal->angle);
+ unsafe_indexer_goal->angular_velocity,
+ unsafe_turret_goal->angle());
- if (unsafe_turret_goal->track) {
+ if (unsafe_turret_goal->track()) {
if (vision_time_adjuster_.valid()) {
AOS_LOG(INFO, "Vision aligning to %f\n",
vision_time_adjuster_.goal());
@@ -623,23 +653,40 @@
// Save debug/internal state.
// TODO(austin): Save more.
- turret_status->zeroed = profiled_subsystem_.zeroed();
- profiled_subsystem_.PopulateTurretStatus(turret_status);
- turret_status->estopped = (state_ == State::ESTOP);
- turret_status->state = static_cast<int32_t>(state_);
- turret_status->turret_encoder_angle = profiled_subsystem_.turret_position();
+ flatbuffers::Offset<ColumnZeroingEstimator::State>
+ column_estimator_state_offset =
+ profiled_subsystem_.EstimatorState(fbb, 0);
+
+ TurretProfiledSubsystemStatus::Builder turret_status_builder(*fbb);
+ profiled_subsystem_.PopulateTurretStatus(&turret_status_builder,
+ column_estimator_state_offset);
+ turret_status_builder.add_estopped((state_ == State::ESTOP));
+ turret_status_builder.add_state(static_cast<int32_t>(state_));
+ turret_status_builder.add_turret_encoder_angle(
+ profiled_subsystem_.turret_position());
if (vision_time_adjuster_.valid()) {
- turret_status->vision_angle = vision_time_adjuster_.goal();
- turret_status->raw_vision_angle =
- vision_time_adjuster_.most_recent_vision_reading();
- turret_status->vision_tracking = true;
+ turret_status_builder.add_vision_angle(vision_time_adjuster_.goal());
+ turret_status_builder.add_raw_vision_angle(
+ vision_time_adjuster_.most_recent_vision_reading());
+ turret_status_builder.add_vision_tracking(true);
} else {
- turret_status->vision_angle = ::std::numeric_limits<double>::quiet_NaN();
- turret_status->vision_tracking = false;
+ turret_status_builder.add_vision_angle(
+ ::std::numeric_limits<double>::quiet_NaN());
+ turret_status_builder.add_vision_tracking(false);
}
- profiled_subsystem_.PopulateIndexerStatus(indexer_status);
- indexer_status->state = static_cast<int32_t>(indexer_state_);
+ flatbuffers::Offset<TurretProfiledSubsystemStatus> turret_status_offset =
+ turret_status_builder.Finish();
+
+ IndexerStatus::Builder indexer_status_builder(*fbb);
+ profiled_subsystem_.PopulateIndexerStatus(&indexer_status_builder);
+
+ indexer_status_builder.add_state(static_cast<int32_t>(indexer_state_));
+
+ flatbuffers::Offset<IndexerStatus> indexer_status_offset =
+ indexer_status_builder.Finish();
+
+ return std::make_pair(indexer_status_offset, turret_status_offset);
}
} // namespace column
diff --git a/y2017/control_loops/superstructure/column/column.h b/y2017/control_loops/superstructure/column/column.h
index 0382059..615565f 100644
--- a/y2017/control_loops/superstructure/column/column.h
+++ b/y2017/control_loops/superstructure/column/column.h
@@ -14,9 +14,10 @@
#include "y2017/constants.h"
#include "y2017/control_loops/superstructure/column/column_zeroing.h"
#include "y2017/control_loops/superstructure/intake/intake.h"
-#include "y2017/control_loops/superstructure/superstructure.q.h"
+#include "y2017/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2017/control_loops/superstructure/superstructure_status_generated.h"
#include "y2017/control_loops/superstructure/vision_time_adjuster.h"
-#include "y2017/vision/vision.q.h"
+#include "y2017/vision/vision_generated.h"
namespace y2017 {
namespace control_loops {
@@ -41,8 +42,10 @@
void Update(bool disabled);
// Fills out the ProfiledJointStatus structure with the current state.
- template <class StatusType>
- void PopulateTurretStatus(StatusType *status);
+ void PopulateTurretStatus(
+ TurretProfiledSubsystemStatus::Builder *status_builder,
+ flatbuffers::Offset<ColumnZeroingEstimator::State>
+ estimator_state_offset);
// Forces the current goal to the provided goal, bypassing the profiler.
void ForceGoal(double goal_velocity, double goal);
@@ -52,7 +55,7 @@
void set_turret_unprofiled_goal(double unprofiled_goal);
void set_unprofiled_goal(double goal_velocity, 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);
@@ -86,7 +89,7 @@
double IndexerStuckVoltage() const;
void PartialIndexerReset();
void PartialTurretReset();
- void PopulateIndexerStatus(IndexerStatus *status);
+ void PopulateIndexerStatus(IndexerStatus::Builder *status_builder);
void AddOffset(double indexer_offset_delta, double turret_offset_delta);
@@ -132,31 +135,6 @@
double turret_last_position_ = 0;
};
-template <typename StatusType>
-void ColumnProfiledSubsystem::PopulateTurretStatus(StatusType *status) {
- status->zeroed = zeroed();
- status->state = -1;
- // We don't know, so default to the bad case.
- status->estopped = true;
-
- status->position = X_hat(2, 0);
- status->velocity = X_hat(3, 0);
- status->goal_position = goal(2, 0);
- status->goal_velocity = goal(3, 0);
- status->unprofiled_goal_position = unprofiled_goal(2, 0);
- status->unprofiled_goal_velocity = unprofiled_goal(3, 0);
- status->voltage_error = X_hat(5, 0);
- status->calculated_velocity =
- (turret_position() - turret_last_position_) /
- ::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency);
-
- status->estimator_state = EstimatorState(0);
-
- Eigen::Matrix<double, 6, 1> error = controller().error();
- status->position_power = controller().controller().K(0, 0) * error(0, 0);
- status->velocity_power = controller().controller().K(0, 1) * error(1, 0);
-}
-
class Column {
public:
Column(::aos::EventLoop *event_loop);
@@ -179,15 +157,16 @@
static constexpr double kTurretMin = -0.1;
static constexpr double kTurretMax = M_PI / 2.0 + 0.1;
- void Iterate(::aos::monotonic_clock::time_point monotonic_now,
- const control_loops::IndexerGoal *unsafe_indexer_goal,
- const control_loops::TurretGoal *unsafe_turret_goal,
- const ColumnPosition *position,
- const vision::VisionStatus *vision_status,
- double *indexer_output, double *turret_output,
- IndexerStatus *indexer_status,
- TurretProfiledSubsystemStatus *turret_status,
- intake::Intake *intake);
+ std::pair<flatbuffers::Offset<IndexerStatus>,
+ flatbuffers::Offset<TurretProfiledSubsystemStatus>>
+ Iterate(::aos::monotonic_clock::time_point monotonic_now,
+ const IndexerGoalT *unsafe_indexer_goal,
+ const TurretGoal *unsafe_turret_goal, const ColumnPosition *position,
+ const vision::VisionStatus *vision_status, double *indexer_output,
+ double *turret_output, flatbuffers::FlatBufferBuilder *fbb,
+ // IndexerStatus *indexer_status,
+ // TurretProfiledSubsystemStatus *turret_status,
+ intake::Intake *intake);
void Reset();
diff --git a/y2017/control_loops/superstructure/column/column_zeroing.cc b/y2017/control_loops/superstructure/column/column_zeroing.cc
index 30500ca..7d5937a 100644
--- a/y2017/control_loops/superstructure/column/column_zeroing.cc
+++ b/y2017/control_loops/superstructure/column/column_zeroing.cc
@@ -31,15 +31,16 @@
}
void ColumnZeroingEstimator::UpdateEstimate(const ColumnPosition &position) {
- indexer_.UpdateEstimate(position.indexer);
- turret_.UpdateEstimate(position.turret);
+ indexer_.UpdateEstimate(*position.indexer());
+ turret_.UpdateEstimate(*position.turret());
if (indexer_.zeroed() && turret_.zeroed()) {
indexer_offset_ = indexer_.offset();
// Compute the current turret position.
- const double current_turret = indexer_offset_ + position.indexer.encoder +
- turret_.offset() + position.turret.encoder;
+ const double current_turret =
+ indexer_offset_ + position.indexer()->encoder() + turret_.offset() +
+ position.turret()->encoder();
// Now, we can compute the turret position which is closest to 0 radians
// (within +- M_PI).
@@ -47,16 +48,16 @@
::frc971::zeroing::Wrap(0.0, current_turret, M_PI * 2.0);
// Now, compute the actual turret offset.
- turret_offset_ = adjusted_turret - position.turret.encoder -
- (indexer_offset_ + position.indexer.encoder);
+ turret_offset_ = adjusted_turret - position.turret()->encoder() -
+ (indexer_offset_ + position.indexer()->encoder());
offset_ready_ = true;
// If we are close enough to 0, we are zeroed. Otherwise, we don't know
// which revolution we are on and need more info. We will always report the
// turret position as within +- M_PI from 0 with the provided offset.
- if (::std::abs(position.indexer.encoder + position.turret.encoder +
- indexer_offset_ + turret_offset_) <
- turret_zeroed_distance_) {
+ if (::std::abs(position.indexer()->encoder() +
+ position.turret()->encoder() + indexer_offset_ +
+ turret_offset_) < turret_zeroed_distance_) {
zeroed_ = true;
}
@@ -65,17 +66,23 @@
}
}
-ColumnZeroingEstimator::State ColumnZeroingEstimator::GetEstimatorState()
- const {
- State r;
- r.error = error();
- r.zeroed = zeroed();
- r.indexer = indexer_.GetEstimatorState();
- r.turret = turret_.GetEstimatorState();
- return r;
+flatbuffers::Offset<ColumnZeroingEstimator::State>
+ColumnZeroingEstimator::GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const {
+ flatbuffers::Offset<frc971::HallEffectAndPositionEstimatorState> indexer_offset =
+ indexer_.GetEstimatorState(fbb);
+ flatbuffers::Offset<frc971::HallEffectAndPositionEstimatorState> turret_offset =
+ turret_.GetEstimatorState(fbb);
+
+ State::Builder state_builder(*fbb);
+ state_builder.add_indexer(indexer_offset);
+ state_builder.add_turret(turret_offset);
+ state_builder.add_error(error());
+ state_builder.add_zeroed(zeroed());
+ return state_builder.Finish();
}
-} // column
-} // superstructure
+} // namespace column
+} // namespace superstructure
} // control_loops
} // y2017
diff --git a/y2017/control_loops/superstructure/column/column_zeroing.h b/y2017/control_loops/superstructure/column/column_zeroing.h
index f2ac94f..8c2cbf3 100644
--- a/y2017/control_loops/superstructure/column/column_zeroing.h
+++ b/y2017/control_loops/superstructure/column/column_zeroing.h
@@ -2,10 +2,10 @@
#define Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_COLUMN_H_
#include "frc971/constants.h"
-#include "frc971/control_loops/control_loops.q.h"
#include "frc971/zeroing/zeroing.h"
#include "y2017/constants.h"
-#include "y2017/control_loops/superstructure/superstructure.q.h"
+#include "y2017/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2017/control_loops/superstructure/superstructure_status_generated.h"
namespace y2017 {
namespace control_loops {
@@ -41,7 +41,8 @@
double turret_offset() const { return turret_offset_; }
// Returns information about our current state.
- State GetEstimatorState() const;
+ flatbuffers::Offset<State> GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const;
private:
// We are ensuring that two subsystems are zeroed, so here they are!
diff --git a/y2017/control_loops/superstructure/column/column_zeroing_test.cc b/y2017/control_loops/superstructure/column/column_zeroing_test.cc
index 3cc9c2e..dab5139 100644
--- a/y2017/control_loops/superstructure/column/column_zeroing_test.cc
+++ b/y2017/control_loops/superstructure/column/column_zeroing_test.cc
@@ -3,10 +3,13 @@
#include <random>
#include "aos/die.h"
+#include "aos/flatbuffers.h"
+#include "aos/json_to_flatbuffer.h"
#include "aos/testing/test_shm.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/control_loops/team_number_test_environment.h"
#include "frc971/zeroing/zeroing.h"
+#include "glog/logging.h"
#include "gtest/gtest.h"
#include "y2017/constants.h"
#include "y2017/control_loops/superstructure/column/column_zeroing.h"
@@ -49,14 +52,28 @@
}
void MoveTo(double indexer, double turret) {
- ColumnPosition column_position;
+ flatbuffers::FlatBufferBuilder fbb;
indexer_sensor_.MoveTo(indexer);
turret_sensor_.MoveTo(turret - indexer);
- indexer_sensor_.GetSensorValues(&column_position.indexer);
- turret_sensor_.GetSensorValues(&column_position.turret);
+ HallEffectAndPosition::Builder indexer_builder(fbb);
+ flatbuffers::Offset<HallEffectAndPosition> indexer_offset =
+ indexer_sensor_.GetSensorValues(&indexer_builder);
- column_zeroing_estimator_.UpdateEstimate(column_position);
+ HallEffectAndPosition::Builder turret_builder(fbb);
+ flatbuffers::Offset<HallEffectAndPosition> turret_offset =
+ turret_sensor_.GetSensorValues(&turret_builder);
+
+ ColumnPosition::Builder column_position_builder(fbb);
+ column_position_builder.add_indexer(indexer_offset);
+ column_position_builder.add_turret(turret_offset);
+ fbb.Finish(column_position_builder.Finish());
+
+
+ aos::FlatbufferDetachedBuffer<ColumnPosition> column_position(fbb.Release());
+ LOG(INFO) << "Position: " << aos::FlatbufferToJson(column_position);
+
+ column_zeroing_estimator_.UpdateEstimate(column_position.message());
}
};