Merge changes I901787ac,I836f4b43
* changes:
Flatbuffers Merge commit '8cd6f0538a362ceefbcfcbf6c7b8b3f341d1fb41' into master
Squashed 'third_party/flatbuffers/' changes from acc9990ab..d6a8dbd26
diff --git a/build_tests/test.fbs b/build_tests/test.fbs
index 71db3e7..ca153ac 100644
--- a/build_tests/test.fbs
+++ b/build_tests/test.fbs
@@ -1,11 +1,11 @@
namespace aos.examples;
table Foo {
- value:int;
+ value:int32 (id: 0);
}
table Bar {
- value:int;
+ value:int32 (id: 0);
}
root_type Foo;
diff --git a/frc971/constants.h b/frc971/constants.h
index 7c2121d..7f1f1d5 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -89,6 +89,33 @@
double allowable_encoder_error;
};
+struct AbsoluteAndAbsoluteEncoderZeroingConstants {
+ // The number of samples in the moving average filter.
+ size_t average_filter_size;
+ // The distance that the absolute encoder needs to complete a full rotation.
+ double one_revolution_distance;
+ // Measured absolute position of the encoder when at zero.
+ double measured_absolute_position;
+
+ // The distance that the single turn absolute encoder needs to complete a full
+ // rotation.
+ double single_turn_one_revolution_distance;
+ // Measured absolute position of the single turn encoder when at zero.
+ double single_turn_measured_absolute_position;
+ // Position of the middle of the range of motion in output coordinates.
+ double single_turn_middle_position;
+
+ // Threshold for deciding if we are moving. moving_buffer_size samples need to
+ // be within this distance of each other before we use the middle one to zero.
+ double zeroing_threshold;
+ // Buffer size for deciding if we are moving.
+ size_t moving_buffer_size;
+
+ // Value between 0 and 1 indicating what fraction of one_revolution_distance
+ // it is acceptable for the offset to move.
+ double allowable_encoder_error;
+};
+
// Defines a range of motion for a subsystem.
// These are all absolute positions in scaled units.
struct Range {
diff --git a/frc971/control_loops/control_loops.fbs b/frc971/control_loops/control_loops.fbs
index 4377541..f510085 100644
--- a/frc971/control_loops/control_loops.fbs
+++ b/frc971/control_loops/control_loops.fbs
@@ -130,6 +130,23 @@
absolute_position:double;
}
+// The internal state of a zeroing estimator.
+table AbsoluteAndAbsoluteEncoderEstimatorState {
+ // If true, there has been a fatal error for the estimator.
+ error:bool (id: 0);
+ // If the joint has seen an index pulse and is zeroed.
+ zeroed:bool (id: 1);
+ // The estimated position of the joint.
+ position:double (id: 2);
+
+ // The estimated absolute position of the encoder. This is filtered, so it
+ // can be easily used when zeroing.
+ absolute_position:double (id: 3);
+
+ // Estimated absolute position of the single turn absolute encoder.
+ single_turn_absolute_position:double (id: 4);
+}
+
table RelativeEncoderEstimatorState {
// If true, there has been a fatal error for the estimator.
error:bool;
diff --git a/frc971/zeroing/BUILD b/frc971/zeroing/BUILD
index 5955f8b..df85290 100644
--- a/frc971/zeroing/BUILD
+++ b/frc971/zeroing/BUILD
@@ -64,6 +64,7 @@
"pot_and_absolute_encoder.cc",
"pot_and_index.cc",
"pulse_index.cc",
+ "absolute_and_absolute_encoder.cc",
],
hdrs = [
"absolute_encoder.h",
@@ -72,6 +73,7 @@
"pot_and_index.h",
"pulse_index.h",
"zeroing.h",
+ "absolute_and_absolute_encoder.h",
],
deps = [
":wrap",
@@ -91,6 +93,7 @@
"pot_and_index_test.cc",
"pulse_index_test.cc",
"relative_encoder_test.cc",
+ "absolute_and_absolute_encoder_test.cc",
"zeroing_test.h",
],
deps = [
diff --git a/frc971/zeroing/absolute_and_absolute_encoder.cc b/frc971/zeroing/absolute_and_absolute_encoder.cc
new file mode 100644
index 0000000..54a5e78
--- /dev/null
+++ b/frc971/zeroing/absolute_and_absolute_encoder.cc
@@ -0,0 +1,216 @@
+#include "frc971/zeroing/absolute_and_absolute_encoder.h"
+
+#include <cmath>
+#include <numeric>
+
+#include "glog/logging.h"
+
+#include "frc971/zeroing/wrap.h"
+
+namespace frc971 {
+namespace zeroing {
+
+AbsoluteAndAbsoluteEncoderZeroingEstimator::
+ AbsoluteAndAbsoluteEncoderZeroingEstimator(
+ const constants::AbsoluteAndAbsoluteEncoderZeroingConstants &constants)
+ : constants_(constants), move_detector_(constants_.moving_buffer_size) {
+ relative_to_absolute_offset_samples_.reserve(constants_.average_filter_size);
+ offset_samples_.reserve(constants_.average_filter_size);
+ Reset();
+}
+
+void AbsoluteAndAbsoluteEncoderZeroingEstimator::Reset() {
+ first_offset_ = 0.0;
+ single_turn_to_relative_encoder_offset_ = 0.0;
+ offset_ = 0.0;
+ samples_idx_ = 0;
+ filtered_position_ = 0.0;
+ position_ = 0.0;
+ zeroed_ = false;
+ nan_samples_ = 0;
+ relative_to_absolute_offset_samples_.clear();
+ offset_samples_.clear();
+ move_detector_.Reset();
+ error_ = false;
+}
+
+// So, this needs to be a multistep process. We need to first estimate the
+// offset between the absolute encoder and the relative encoder. That process
+// should get us an absolute number which is off by integer multiples of the
+// distance/rev. In parallel, we can estimate the offset between the single
+// turn encoder and encoder. When both estimates have converged, we can then
+// compute the offset in a cycle, and which cycle, which gives us the accurate
+// global offset.
+//
+// It's tricky to compute the offset between the absolute and relative encoder.
+// We need to compute this inside 1 revolution. The easiest way to do this
+// would be to wrap the encoder, subtract the two of them, and then average the
+// result. That will struggle when they are off by PI. Instead, we need to
+// wrap the number to +- PI from the current averaged offset.
+//
+// To guard against the robot moving while updating estimates, buffer a number
+// of samples and check that the buffered samples are not different than the
+// zeroing threshold. At any point that the samples differ too much, do not
+// update estimates based on those samples.
+void AbsoluteAndAbsoluteEncoderZeroingEstimator::UpdateEstimate(
+ const AbsoluteAndAbsolutePosition &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 (zeroed_) {
+ VLOG(1) << "NAN on absolute encoder.";
+ error_ = true;
+ } else {
+ ++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_ =
+ single_turn_to_relative_encoder_offset_ + info.encoder();
+ position_ = offset_ + info.encoder();
+ return;
+ }
+
+ const bool moving = move_detector_.Update(info, constants_.moving_buffer_size,
+ constants_.zeroing_threshold);
+
+ if (!moving) {
+ 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.
+ double average_relative_to_absolute_offset =
+ relative_to_absolute_offset_samples_.size() == 0
+ ? 0.0
+ : ::std::accumulate(relative_to_absolute_offset_samples_.begin(),
+ relative_to_absolute_offset_samples_.end(),
+ 0.0) /
+ relative_to_absolute_offset_samples_.size();
+
+ const double adjusted_incremental_encoder =
+ sample.encoder + average_relative_to_absolute_offset;
+
+ // Now, compute the nearest absolute encoder value to the offset relative
+ // encoder position.
+ const double adjusted_absolute_encoder =
+ UnWrap(adjusted_incremental_encoder,
+ sample.absolute_encoder - constants_.measured_absolute_position,
+ constants_.one_revolution_distance);
+
+ // We can now compute the offset now that we have unwrapped the absolute
+ // encoder.
+ const double relative_to_absolute_offset =
+ adjusted_absolute_encoder - sample.encoder;
+
+ // Add the sample and update the average with the new reading.
+ const size_t relative_to_absolute_offset_samples_size =
+ relative_to_absolute_offset_samples_.size();
+ if (relative_to_absolute_offset_samples_size <
+ constants_.average_filter_size) {
+ average_relative_to_absolute_offset =
+ (average_relative_to_absolute_offset *
+ relative_to_absolute_offset_samples_size +
+ relative_to_absolute_offset) /
+ (relative_to_absolute_offset_samples_size + 1);
+
+ relative_to_absolute_offset_samples_.push_back(
+ relative_to_absolute_offset);
+ } else {
+ average_relative_to_absolute_offset -=
+ relative_to_absolute_offset_samples_[samples_idx_] /
+ relative_to_absolute_offset_samples_size;
+ relative_to_absolute_offset_samples_[samples_idx_] =
+ relative_to_absolute_offset;
+ average_relative_to_absolute_offset +=
+ relative_to_absolute_offset /
+ relative_to_absolute_offset_samples_size;
+ }
+
+ const double adjusted_single_turn_absolute_encoder =
+ UnWrap(constants_.single_turn_middle_position,
+ sample.single_turn_absolute_encoder -
+ constants_.single_turn_measured_absolute_position,
+ constants_.single_turn_one_revolution_distance);
+
+ // Now compute the offset between the pot and relative encoder.
+ if (offset_samples_.size() < constants_.average_filter_size) {
+ offset_samples_.push_back(adjusted_single_turn_absolute_encoder -
+ sample.encoder);
+ } else {
+ offset_samples_[samples_idx_] =
+ adjusted_single_turn_absolute_encoder - sample.encoder;
+ }
+
+ // Drop the oldest sample when we run this function the next time around.
+ samples_idx_ = (samples_idx_ + 1) % constants_.average_filter_size;
+
+ single_turn_to_relative_encoder_offset_ =
+ ::std::accumulate(offset_samples_.begin(), offset_samples_.end(), 0.0) /
+ offset_samples_.size();
+
+ offset_ = UnWrap(sample.encoder + single_turn_to_relative_encoder_offset_,
+ average_relative_to_absolute_offset + sample.encoder,
+ constants_.one_revolution_distance) -
+ sample.encoder;
+
+ // Reverse the math for adjusted_absolute_encoder to compute the absolute
+ // encoder. Do this by taking the adjusted encoder, and then subtracting off
+ // the second argument above, and the value that was added by Wrap.
+ filtered_absolute_encoder_ =
+ ((sample.encoder + average_relative_to_absolute_offset) -
+ (-constants_.measured_absolute_position +
+ (adjusted_absolute_encoder -
+ (sample.absolute_encoder - constants_.measured_absolute_position))));
+
+ // TODO(Ravago): this is impossible to read.
+ filtered_single_turn_absolute_encoder_ =
+ ((sample.encoder + single_turn_to_relative_encoder_offset_) -
+ (-constants_.single_turn_measured_absolute_position +
+ (adjusted_single_turn_absolute_encoder -
+ (sample.single_turn_absolute_encoder -
+ constants_.single_turn_measured_absolute_position))));
+
+ if (offset_ready()) {
+ if (!zeroed_) {
+ first_offset_ = offset_;
+ }
+
+ if (::std::abs(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;
+ }
+
+ zeroed_ = true;
+ }
+ }
+
+ // Update the position.
+ filtered_position_ = single_turn_to_relative_encoder_offset_ + info.encoder();
+ position_ = offset_ + info.encoder();
+}
+
+flatbuffers::Offset<AbsoluteAndAbsoluteEncoderZeroingEstimator::State>
+AbsoluteAndAbsoluteEncoderZeroingEstimator::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_);
+ builder.add_single_turn_absolute_position(
+ filtered_single_turn_absolute_encoder_);
+ return builder.Finish();
+}
+
+} // namespace zeroing
+} // namespace frc971
diff --git a/frc971/zeroing/absolute_and_absolute_encoder.h b/frc971/zeroing/absolute_and_absolute_encoder.h
new file mode 100644
index 0000000..7d56f2b
--- /dev/null
+++ b/frc971/zeroing/absolute_and_absolute_encoder.h
@@ -0,0 +1,110 @@
+#ifndef frc971_zeroing_absolute_and_absolute_encoder_h_
+#define frc971_zeroing_absolute_and_absolute_encoder_h_
+
+#include <vector>
+
+#include "flatbuffers/flatbuffers.h"
+
+#include "frc971/zeroing/zeroing.h"
+
+namespace frc971 {
+namespace zeroing {
+
+// Estimates the position with an absolute encoder which also reports
+// incremental counts and an absolute encoder that's not allowed to turn more
+// than once. This is like the PotAndAbsoluteEncoderZeroingEstimator, but it
+// uses the single turn absolute encoder instead of the potentiometer.
+class AbsoluteAndAbsoluteEncoderZeroingEstimator
+ : public ZeroingEstimator<
+ AbsoluteAndAbsolutePosition,
+ constants::AbsoluteAndAbsoluteEncoderZeroingConstants,
+ AbsoluteAndAbsoluteEncoderEstimatorState> {
+ public:
+ explicit AbsoluteAndAbsoluteEncoderZeroingEstimator(
+ const constants::AbsoluteAndAbsoluteEncoderZeroingConstants &constants);
+
+ // Resets the internal logic so it needs to be re-zeroed.
+ void Reset() override;
+
+ // Updates the sensor values for the zeroing logic.
+ void UpdateEstimate(const AbsoluteAndAbsolutePosition &info) override;
+
+ void TriggerError() override { error_ = true; }
+
+ bool zeroed() const override { return zeroed_; }
+
+ double offset() const override { return offset_; }
+
+ bool error() const override { return error_; }
+
+ // Returns true if the sample buffer is full.
+ bool offset_ready() const override {
+ return relative_to_absolute_offset_samples_.size() ==
+ constants_.average_filter_size &&
+ offset_samples_.size() == constants_.average_filter_size;
+ }
+
+ // Returns information about our current state.
+ virtual flatbuffers::Offset<State> GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const override;
+
+ private:
+ struct PositionStruct {
+ PositionStruct(const AbsoluteAndAbsolutePosition &position_buffer)
+ : single_turn_absolute_encoder(
+ position_buffer.single_turn_absolute_encoder()),
+ absolute_encoder(position_buffer.absolute_encoder()),
+ encoder(position_buffer.encoder()) {}
+ double single_turn_absolute_encoder;
+ double absolute_encoder;
+ double encoder;
+ };
+
+ // The zeroing constants used to describe the configuration of the system.
+ const constants::AbsoluteAndAbsoluteEncoderZeroingConstants constants_;
+
+ // True if the mechanism is zeroed.
+ bool zeroed_;
+ // Marker to track whether an error has occurred.
+ bool error_;
+ // The first valid offset we recorded. This is only set after zeroed_ first
+ // changes to true.
+ double first_offset_;
+
+ // The filtered absolute encoder. This is used in the status for calibration.
+ double filtered_absolute_encoder_ = 0.0;
+
+ // The filtered single turn absolute encoder. This is used in the status for
+ // calibration.
+ double filtered_single_turn_absolute_encoder_ = 0.0;
+
+ // Samples of the offset needed to line the relative encoder up with the
+ // absolute encoder.
+ ::std::vector<double> relative_to_absolute_offset_samples_;
+
+ // Offset between the single turn absolute encoder and relative encoder
+ // position.
+ ::std::vector<double> offset_samples_;
+
+ MoveDetector<PositionStruct, AbsoluteAndAbsolutePosition> move_detector_;
+
+ // Estimated offset between the single turn encoder and relative encoder.
+ double single_turn_to_relative_encoder_offset_ = 0;
+ // Estimated start position of the mechanism
+ double offset_ = 0;
+ // The next position in 'relative_to_absolute_offset_samples_' and
+ // 'encoder_samples_' to be used to store the next sample.
+ int samples_idx_ = 0;
+
+ size_t nan_samples_ = 0;
+
+ // The unzeroed filtered position.
+ double filtered_position_ = 0.0;
+ // The filtered position.
+ double position_ = 0.0;
+};
+
+} // namespace zeroing
+} // namespace frc971
+
+#endif // FRC971_ZEROING_ABSOLUTE_AND_ABSOLUTE_ENCODER_H_
diff --git a/frc971/zeroing/absolute_and_absolute_encoder_test.cc b/frc971/zeroing/absolute_and_absolute_encoder_test.cc
new file mode 100644
index 0000000..cc1872d
--- /dev/null
+++ b/frc971/zeroing/absolute_and_absolute_encoder_test.cc
@@ -0,0 +1,258 @@
+#include "frc971/zeroing/absolute_and_absolute_encoder.h"
+
+#include "gtest/gtest.h"
+
+#include "frc971/zeroing/zeroing_test.h"
+
+namespace frc971 {
+namespace zeroing {
+namespace testing {
+
+using constants::AbsoluteAndAbsoluteEncoderZeroingConstants;
+using EstimatorState = AbsoluteAndAbsoluteEncoderZeroingEstimator::State;
+
+class AbsoluteAndAbsoluteEncoderZeroingTest : public ZeroingTest {
+ protected:
+ void MoveTo(PositionSensorSimulator *simulator,
+ AbsoluteAndAbsoluteEncoderZeroingEstimator *estimator,
+ double new_position) {
+ simulator->MoveTo(new_position);
+ FBB fbb;
+ estimator->UpdateEstimate(
+ *simulator->FillSensorValues<AbsoluteAndAbsolutePosition>(&fbb));
+ }
+};
+
+// Makes sure that using an absolute encoder lets us zero without moving.
+TEST_F(AbsoluteAndAbsoluteEncoderZeroingTest,
+ TestAbsoluteAndAbsoluteEncoderZeroingWithoutMovement) {
+ const double full_range = 4.0;
+
+ const double distance_per_revolution = 1.0;
+ const double single_turn_distance_per_revolution = full_range;
+
+ const double start_pos = 2.1;
+
+ // Middle position for the single turn absolute encoder.
+ const double single_turn_middle_position = full_range * 0.5;
+
+ const double measured_absolute_position = 0.3 * distance_per_revolution;
+ const double single_turn_measured_absolute_position =
+ 0.4 * single_turn_distance_per_revolution;
+
+ AbsoluteAndAbsoluteEncoderZeroingConstants constants{
+ kSampleSize,
+ distance_per_revolution,
+ measured_absolute_position,
+ single_turn_distance_per_revolution,
+ single_turn_measured_absolute_position,
+ single_turn_middle_position,
+ 0.1,
+ kMovingBufferSize,
+ kIndexErrorFraction};
+
+ PositionSensorSimulator sim(distance_per_revolution,
+ single_turn_distance_per_revolution);
+ sim.Initialize(start_pos, distance_per_revolution / 3.0, 0.0,
+ measured_absolute_position,
+ single_turn_measured_absolute_position);
+
+ AbsoluteAndAbsoluteEncoderZeroingEstimator estimator(constants);
+
+ for (size_t i = 0; i < kSampleSize + kMovingBufferSize - 1; ++i) {
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_FALSE(estimator.zeroed());
+ }
+
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_TRUE(estimator.zeroed());
+ EXPECT_DOUBLE_EQ(start_pos, estimator.offset());
+}
+
+// Makes sure that we ignore a NAN if we get it, but will correctly zero
+// afterwards.
+TEST_F(AbsoluteAndAbsoluteEncoderZeroingTest,
+ TestAbsoluteAndAbsoluteEncoderZeroingIgnoresNAN) {
+ const double full_range = 4.0;
+
+ const double distance_per_revolution = 1.0;
+ const double single_turn_distance_per_revolution = full_range;
+
+ const double start_pos = 2.1;
+
+ // Middle position for the single turn absolute encoder.
+ const double single_turn_middle_position = full_range * 0.5;
+
+ const double measured_absolute_position = 0.3 * distance_per_revolution;
+ const double single_turn_measured_absolute_position =
+ 0.4 * single_turn_distance_per_revolution;
+
+ AbsoluteAndAbsoluteEncoderZeroingConstants constants{
+ kSampleSize,
+ distance_per_revolution,
+ measured_absolute_position,
+ single_turn_distance_per_revolution,
+ single_turn_measured_absolute_position,
+ single_turn_middle_position,
+ 0.1,
+ kMovingBufferSize,
+ kIndexErrorFraction};
+
+ PositionSensorSimulator sim(distance_per_revolution,
+ single_turn_distance_per_revolution);
+ sim.Initialize(start_pos, distance_per_revolution / 3.0, 0.0,
+ measured_absolute_position,
+ single_turn_measured_absolute_position);
+
+ AbsoluteAndAbsoluteEncoderZeroingEstimator estimator(constants);
+
+ // We tolerate a couple NANs before we start.
+ FBB fbb;
+ fbb.Finish(CreateAbsoluteAndAbsolutePosition(
+ fbb, 0.0, ::std::numeric_limits<double>::quiet_NaN(), 0.0));
+ for (size_t i = 0; i < kSampleSize - 1; ++i) {
+ estimator.UpdateEstimate(*flatbuffers::GetRoot<AbsoluteAndAbsolutePosition>(
+ fbb.GetBufferPointer()));
+ }
+
+ for (size_t i = 0; i < kSampleSize + kMovingBufferSize - 1; ++i) {
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_FALSE(estimator.zeroed());
+ }
+
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_TRUE(estimator.zeroed());
+ EXPECT_DOUBLE_EQ(start_pos, estimator.offset());
+}
+
+// Makes sure that using an absolute encoder doesn't let us zero while moving.
+TEST_F(AbsoluteAndAbsoluteEncoderZeroingTest,
+ TestAbsoluteAndAbsoluteEncoderZeroingWithMovement) {
+ const double full_range = 4.0;
+
+ const double distance_per_revolution = 1.0;
+ const double single_turn_distance_per_revolution = full_range;
+
+ const double start_pos = 2.1;
+
+ // Middle position for the single turn absolute encoder.
+ const double single_turn_middle_position = full_range * 0.5;
+
+ const double measured_absolute_position = 0.3 * distance_per_revolution;
+ const double single_turn_measured_absolute_position =
+ 0.4 * single_turn_distance_per_revolution;
+
+ AbsoluteAndAbsoluteEncoderZeroingConstants constants{
+ kSampleSize,
+ distance_per_revolution,
+ measured_absolute_position,
+ single_turn_distance_per_revolution,
+ single_turn_measured_absolute_position,
+ single_turn_middle_position,
+ 0.1,
+ kMovingBufferSize,
+ kIndexErrorFraction};
+
+ PositionSensorSimulator sim(distance_per_revolution,
+ single_turn_distance_per_revolution);
+ sim.Initialize(start_pos, distance_per_revolution / 3.0, 0.0,
+ measured_absolute_position,
+ single_turn_measured_absolute_position);
+
+ AbsoluteAndAbsoluteEncoderZeroingEstimator estimator(constants);
+
+ for (size_t i = 0; i < kSampleSize + kMovingBufferSize - 1; ++i) {
+ MoveTo(&sim, &estimator, start_pos + i * distance_per_revolution);
+ ASSERT_FALSE(estimator.zeroed());
+ }
+ MoveTo(&sim, &estimator, start_pos + 10 * distance_per_revolution);
+
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_FALSE(estimator.zeroed());
+}
+
+// Makes sure we detect an error if the ZeroingEstimator gets sent a NaN.
+TEST_F(AbsoluteAndAbsoluteEncoderZeroingTest,
+ TestAbsoluteAndAbsoluteEncoderZeroingWithNaN) {
+ AbsoluteAndAbsoluteEncoderZeroingConstants constants{
+ kSampleSize, 1, 0.3, 1, 0.3, 2.5, 0.1, kMovingBufferSize,
+ kIndexErrorFraction};
+
+ AbsoluteAndAbsoluteEncoderZeroingEstimator estimator(constants);
+
+ FBB fbb;
+ fbb.Finish(CreateAbsoluteAndAbsolutePosition(
+ fbb, 0.0, ::std::numeric_limits<double>::quiet_NaN(), 0.0));
+ const auto sensor_values =
+ flatbuffers::GetRoot<AbsoluteAndAbsolutePosition>(fbb.GetBufferPointer());
+ for (size_t i = 0; i < kSampleSize - 1; ++i) {
+ estimator.UpdateEstimate(*sensor_values);
+ }
+ ASSERT_FALSE(estimator.error());
+
+ estimator.UpdateEstimate(*sensor_values);
+ ASSERT_TRUE(estimator.error());
+}
+
+TEST_F(AbsoluteAndAbsoluteEncoderZeroingTest,
+ TestAbsoluteAndAbsoluteEncoderZeroingState) {
+ const double full_range = 4.0;
+ const double distance_per_revolution = 1.0;
+ const double single_turn_distance_per_revolution = full_range;
+ const double start_pos = 2.1;
+ const double single_turn_middle_position = full_range * 0.5;
+ const double measured_absolute_position = 0.3 * distance_per_revolution;
+ const double single_turn_measured_absolute_position =
+ 0.4 * single_turn_distance_per_revolution;
+
+ AbsoluteAndAbsoluteEncoderZeroingConstants constants{
+ kSampleSize,
+ distance_per_revolution,
+ measured_absolute_position,
+ single_turn_distance_per_revolution,
+ single_turn_measured_absolute_position,
+ single_turn_middle_position,
+ 0.1,
+ kMovingBufferSize,
+ kIndexErrorFraction};
+
+ PositionSensorSimulator sim(distance_per_revolution,
+ single_turn_distance_per_revolution);
+ sim.Initialize(start_pos, distance_per_revolution / 3.0, 0.0,
+ measured_absolute_position,
+ single_turn_measured_absolute_position);
+
+ AbsoluteAndAbsoluteEncoderZeroingEstimator estimator(constants);
+
+ const double position = 2.7;
+
+ for (size_t i = 0; i < kSampleSize + kMovingBufferSize - 1; ++i) {
+ MoveTo(&sim, &estimator, position);
+ ASSERT_FALSE(estimator.zeroed());
+ }
+ MoveTo(&sim, &estimator, position);
+ ASSERT_TRUE(estimator.zeroed());
+ EXPECT_DOUBLE_EQ(start_pos, estimator.offset());
+
+ FBB fbb;
+
+ fbb.Finish(estimator.GetEstimatorState(&fbb));
+
+ const EstimatorState *state =
+ flatbuffers::GetRoot<EstimatorState>(fbb.GetBufferPointer());
+
+ EXPECT_NEAR(state->position(), position, 1e-10);
+
+ // (position + measured_absolute_position) % distance_per_revolution
+ // (2.7 + 0.3) % 1
+ EXPECT_NEAR(state->absolute_position(), 0.0, 1e-10);
+
+ // (position + single_turn_measured_absolute_position) %
+ // single_turn_distance_per_revolution
+ // (2.7 + 1.6) % 4
+ EXPECT_NEAR(state->single_turn_absolute_position(), 0.3, 1e-10);
+}
+
+} // namespace testing
+} // namespace zeroing
+} // namespace frc971
diff --git a/y2014/control_loops/claw/claw_goal.fbs b/y2014/control_loops/claw/claw_goal.fbs
index eb5860c..8db5e68 100644
--- a/y2014/control_loops/claw/claw_goal.fbs
+++ b/y2014/control_loops/claw/claw_goal.fbs
@@ -3,13 +3,13 @@
// All angles here are 0 vertical, positive "up" (aka backwards).
table Goal {
// The angle of the bottom claw.
- bottom_angle:double;
+ bottom_angle:double (id: 0);
// How much higher the top claw is.
- separation_angle:double;
+ separation_angle:double (id: 1);
// top claw intake roller
- intake:double;
+ intake:double (id: 2);
// bottom claw tusk centering
- centering:double;
+ centering:double (id: 3);
}
root_type Goal;
diff --git a/y2014/control_loops/claw/claw_output.fbs b/y2014/control_loops/claw/claw_output.fbs
index 8cb42e0..d0a6b58 100644
--- a/y2014/control_loops/claw/claw_output.fbs
+++ b/y2014/control_loops/claw/claw_output.fbs
@@ -2,10 +2,10 @@
// All angles here are 0 vertical, positive "up" (aka backwards).
table Output {
- intake_voltage:double;
- top_claw_voltage:double;
- bottom_claw_voltage:double;
- tusk_voltage:double;
+ intake_voltage:double (id: 0);
+ top_claw_voltage:double (id: 1);
+ bottom_claw_voltage:double (id: 2);
+ tusk_voltage:double (id: 3);
}
root_type Output;
diff --git a/y2014/control_loops/claw/claw_position.fbs b/y2014/control_loops/claw/claw_position.fbs
index 17d9f17..2a8eb8a 100644
--- a/y2014/control_loops/claw/claw_position.fbs
+++ b/y2014/control_loops/claw/claw_position.fbs
@@ -5,21 +5,21 @@
// All angles here are 0 vertical, positive "up" (aka backwards).
table HalfClawPosition {
// The current position of this half of the claw.
- position:double;
+ position:double (id: 0);
// The hall effect sensor at the front limit.
- front:frc971.HallEffectStruct;
+ front:frc971.HallEffectStruct (id: 1);
// The hall effect sensor in the middle to use for real calibration.
- calibration:frc971.HallEffectStruct;
+ calibration:frc971.HallEffectStruct (id: 2);
// The hall effect at the back limit.
- back:frc971.HallEffectStruct;
+ back:frc971.HallEffectStruct (id: 3);
}
table Position {
// All the top claw information.
- top:HalfClawPosition;
+ top:HalfClawPosition (id: 0);
// All the bottom claw information.
- bottom:HalfClawPosition;
+ bottom:HalfClawPosition (id: 1);
}
root_type Position;
diff --git a/y2014/control_loops/claw/claw_status.fbs b/y2014/control_loops/claw/claw_status.fbs
index eda9279..7d502cc 100644
--- a/y2014/control_loops/claw/claw_status.fbs
+++ b/y2014/control_loops/claw/claw_status.fbs
@@ -3,19 +3,19 @@
// All angles here are 0 vertical, positive "up" (aka backwards).
table Status {
// True if zeroed enough for the current period (autonomous or teleop).
- zeroed:bool;
+ zeroed:bool (id: 0);
// True if zeroed as much as we will force during autonomous.
- zeroed_for_auto:bool;
+ zeroed_for_auto:bool (id: 1);
// True if zeroed and within tolerance for separation and bottom angle.
- done:bool;
+ done:bool (id: 2);
// True if zeroed and within tolerance for separation and bottom angle.
// seperation allowance much wider as a ball may be included
- done_with_ball:bool;
+ done_with_ball:bool (id: 3);
// Dump the values of the state matrix.
- bottom:double;
- bottom_velocity:double;
- separation:double;
- separation_velocity:double;
+ bottom:double (id: 4);
+ bottom_velocity:double (id: 5);
+ separation:double (id: 6);
+ separation_velocity:double (id: 7);
}
root_type Status;
diff --git a/y2014/control_loops/shooter/shooter_goal.fbs b/y2014/control_loops/shooter/shooter_goal.fbs
index 3326562..5f009e7 100644
--- a/y2014/control_loops/shooter/shooter_goal.fbs
+++ b/y2014/control_loops/shooter/shooter_goal.fbs
@@ -2,11 +2,11 @@
table Goal {
// Shot power in joules.
- shot_power:double;
+ shot_power:double (id: 0);
// Shoots as soon as this is true.
- shot_requested:bool;
- unload_requested:bool;
- load_requested:bool;
+ shot_requested:bool (id: 1);
+ unload_requested:bool (id: 2);
+ load_requested:bool (id: 3);
}
root_type Goal;
diff --git a/y2014/control_loops/shooter/shooter_output.fbs b/y2014/control_loops/shooter/shooter_output.fbs
index e1900c4..eaf0713 100644
--- a/y2014/control_loops/shooter/shooter_output.fbs
+++ b/y2014/control_loops/shooter/shooter_output.fbs
@@ -1,11 +1,11 @@
namespace y2014.control_loops.shooter;
table Output {
- voltage:double;
+ voltage:double (id: 0);
// true: latch engaged, false: latch open
- latch_piston:bool;
+ latch_piston:bool (id: 1);
// true: brake engaged false: brake released
- brake_piston:bool;
+ brake_piston:bool (id: 2);
}
root_type Output;
diff --git a/y2014/control_loops/shooter/shooter_position.fbs b/y2014/control_loops/shooter/shooter_position.fbs
index 9c73a1a..50a6129 100644
--- a/y2014/control_loops/shooter/shooter_position.fbs
+++ b/y2014/control_loops/shooter/shooter_position.fbs
@@ -5,17 +5,17 @@
// Back is when the springs are all the way stretched.
table Position {
// In meters, out is positive.
- position:double;
+ position:double (id: 0);
// If the latch piston is fired and this hall effect has been triggered, the
// plunger is all the way back and latched.
- plunger:bool;
+ plunger:bool (id: 1);
// Gets triggered when the pusher is all the way back.
- pusher_distal:frc971.PosedgeOnlyCountedHallEffectStruct;
+ pusher_distal:frc971.PosedgeOnlyCountedHallEffectStruct (id: 2);
// Triggers just before pusher_distal.
- pusher_proximal:frc971.PosedgeOnlyCountedHallEffectStruct;
+ pusher_proximal:frc971.PosedgeOnlyCountedHallEffectStruct (id: 3);
// Triggers when the latch engages.
- latch:bool;
+ latch:bool (id: 4);
}
root_type Position;
diff --git a/y2014/control_loops/shooter/shooter_status.fbs b/y2014/control_loops/shooter/shooter_status.fbs
index 4e76e27..ba1969d 100644
--- a/y2014/control_loops/shooter/shooter_status.fbs
+++ b/y2014/control_loops/shooter/shooter_status.fbs
@@ -2,20 +2,20 @@
table Status {
// Whether it's ready to shoot right now.
- ready:bool;
+ ready:bool (id: 0);
// Whether the plunger is in and out of the way of grabbing a ball.
// TODO(ben): Populate these!
//cocked:bool;
// How many times we've shot.
- shots:int;
+ shots:int32 (id: 1);
//done:bool;
// What we think the current position of the hard stop on the shooter is, in
// shot power (Joules).
- hard_stop_power:double;
+ hard_stop_power:double (id: 2);
- absolute_position:double;
- absolute_velocity:double;
- state:uint;
+ absolute_position:double (id: 3);
+ absolute_velocity:double (id: 4);
+ state:uint32 (id: 5);
}
root_type Status;
diff --git a/y2014/queues/auto_mode.fbs b/y2014/queues/auto_mode.fbs
index bca34f8..c9e6a65 100644
--- a/y2014/queues/auto_mode.fbs
+++ b/y2014/queues/auto_mode.fbs
@@ -3,7 +3,7 @@
// Published on "/aos"
table AutoMode {
// Voltage of the analog auto selector knob.
- voltage:double;
+ voltage:double (id: 0);
}
root_type AutoMode;
diff --git a/y2014/queues/hot_goal.fbs b/y2014/queues/hot_goal.fbs
index 5c4bd0d..527567b 100644
--- a/y2014/queues/hot_goal.fbs
+++ b/y2014/queues/hot_goal.fbs
@@ -2,8 +2,8 @@
// Published on "/"
table HotGoal {
- left_count:ulong;
- right_count:ulong;
+ left_count:uint64 (id: 0);
+ right_count:uint64 (id: 1);
}
root_type HotGoal;
diff --git a/y2014_bot3/control_loops/rollers/rollers_goal.fbs b/y2014_bot3/control_loops/rollers/rollers_goal.fbs
index 92ee589..3986d1f 100644
--- a/y2014_bot3/control_loops/rollers/rollers_goal.fbs
+++ b/y2014_bot3/control_loops/rollers/rollers_goal.fbs
@@ -2,11 +2,11 @@
table Goal {
// -1 = back intake, 1 = front intake, all else = stationary.
- intake:int;
+ intake:int32 (id: 0);
// -1 = backwards, 1 = forwards, all else = stationary.
- low_spit:int;
+ low_spit:int (id: 1);
// Whether we want the human player load function.
- human_player:bool;
+ human_player:bool (id: 2);
}
root_type Goal;
diff --git a/y2014_bot3/control_loops/rollers/rollers_output.fbs b/y2014_bot3/control_loops/rollers/rollers_output.fbs
index daae09b..c1d023e 100644
--- a/y2014_bot3/control_loops/rollers/rollers_output.fbs
+++ b/y2014_bot3/control_loops/rollers/rollers_output.fbs
@@ -2,15 +2,15 @@
table Output {
// Positive voltage = intaking, Negative = spitting.
- front_intake_voltage:double;
- back_intake_voltage:double;
+ front_intake_voltage:double (id: 0);
+ back_intake_voltage:double (id: 1);
// Voltage for the low goal rollers.
// Positive voltage = ball towards back, Negative = ball towards front.
- low_goal_voltage:double;
+ low_goal_voltage:double (id: 2);
// Whether the front and back intake pistons are extended.
- front_extended:bool;
- back_extended:bool;
+ front_extended:bool (id: 3);
+ back_extended:bool (id: 4);
}
root_type Output;