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/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());
}