Merge "Remove unused, incorrect, duplicate constant"
diff --git a/aos/vision/blob/BUILD b/aos/vision/blob/BUILD
index f823cc0..cf728c5 100644
--- a/aos/vision/blob/BUILD
+++ b/aos/vision/blob/BUILD
@@ -94,6 +94,43 @@
],
)
+cc_library(
+ name = 'move_scale',
+ hdrs = ['move_scale.h'],
+ srcs = ['move_scale.cc'],
+ deps = [
+ ':range_image',
+ ],
+)
+
+cc_library(
+ name = 'test_utils',
+ hdrs = ['test_utils.h'],
+ srcs = ['test_utils.cc'],
+ deps = [
+ ':range_image',
+ ],
+)
+
+cc_library(
+ name = 'transpose',
+ hdrs = ['transpose.h'],
+ srcs = ['transpose.cc'],
+ deps = [
+ ':range_image',
+ ],
+)
+
+cc_test(
+ name = 'transpose_test',
+ srcs = ['transpose_test.cc'],
+ deps = [
+ ':transpose',
+ ':test_utils',
+ '//aos/testing:googletest',
+ ],
+)
+
gtk_dependent_cc_library(
name = 'stream_view',
hdrs = ['stream_view.h'],
diff --git a/aos/vision/blob/codec.h b/aos/vision/blob/codec.h
index c2bc30d..b8a8089 100644
--- a/aos/vision/blob/codec.h
+++ b/aos/vision/blob/codec.h
@@ -12,11 +12,12 @@
struct IntCodec {
static constexpr size_t kSize = sizeof(T);
static inline char *Write(char *data, T ival) {
- *(reinterpret_cast<T *>(data)) = ival;
+ memcpy(data, &ival, sizeof(T));
return data + kSize;
}
static inline T Read(const char **data) {
- auto datum = *(reinterpret_cast<const T *>(*data));
+ T datum;
+ memcpy(&datum, *data, sizeof(T));
*data += kSize;
return datum;
}
diff --git a/aos/vision/blob/find_blob.cc b/aos/vision/blob/find_blob.cc
index 0e2586b..3153fcb 100644
--- a/aos/vision/blob/find_blob.cc
+++ b/aos/vision/blob/find_blob.cc
@@ -52,7 +52,7 @@
};
// Uses disjoint set class to track range images.
-// Joins in the disjoint set are done at the same time as joins in the
+// Joins in the disjoint set are done at the same time as joins in the
// range image.
class BlobDisjointSet {
public:
diff --git a/aos/vision/blob/hierarchical_contour_merge.cc b/aos/vision/blob/hierarchical_contour_merge.cc
index b3d9f41..ec617ca 100644
--- a/aos/vision/blob/hierarchical_contour_merge.cc
+++ b/aos/vision/blob/hierarchical_contour_merge.cc
@@ -22,7 +22,7 @@
// This is an exclusive range lookup into a modulo ring.
// The integral is precomputed in items_ and is inclusive even though
- // the input is [a, b).
+ // the input is [a, b).
T Get(int a, int b) {
a = Mod(a, items_.size());
b = Mod(b, items_.size());
diff --git a/aos/vision/blob/move_scale.cc b/aos/vision/blob/move_scale.cc
new file mode 100644
index 0000000..1caa479
--- /dev/null
+++ b/aos/vision/blob/move_scale.cc
@@ -0,0 +1,42 @@
+#include "aos/vision/blob/move_scale.h"
+
+namespace aos {
+namespace vision {
+
+RangeImage MoveScale(const RangeImage &img, int dx, int dy, int scale) {
+ std::vector<std::vector<ImageRange>> out_range_list;
+ for (const auto &range_list : img) {
+ std::vector<ImageRange> ranges;
+ for (const auto &range : range_list) {
+ ranges.emplace_back((range.st + dx) * scale, (range.ed + dx) * scale);
+ }
+ for (int i = 1; i < scale; ++i) {
+ out_range_list.push_back(ranges);
+ }
+ out_range_list.emplace_back(std::move(ranges));
+ }
+ return RangeImage((img.mini() + dy) * scale, std::move(out_range_list));
+}
+
+std::vector<RangeImage> MoveScale(const std::vector<RangeImage> &imgs, int dx,
+ int dy, int scale) {
+ std::vector<RangeImage> out;
+ for (const auto &img : imgs) {
+ out.emplace_back(MoveScale(img, dx, dy, scale));
+ }
+ return out;
+}
+
+void GetBBox(const RangeImage &img, ImageBBox *bbox) {
+ bbox->miny = std::min(img.min_y(), bbox->miny);
+ bbox->maxy = std::max(img.min_y() + img.size(), bbox->maxy);
+ for (const auto &range_lst : img) {
+ if (range_lst.size() > 0) {
+ bbox->maxx = std::max(range_lst[range_lst.size() - 1].ed, bbox->maxx);
+ bbox->minx = std::min(range_lst[0].st, bbox->minx);
+ }
+ }
+}
+
+} // namespace vision
+} // namespace aos
diff --git a/aos/vision/blob/move_scale.h b/aos/vision/blob/move_scale.h
new file mode 100644
index 0000000..c113bca
--- /dev/null
+++ b/aos/vision/blob/move_scale.h
@@ -0,0 +1,35 @@
+#ifndef AOS_VISION_BLOB_MOVE_SCALE_H_
+#define AOS_VISION_BLOB_MOVE_SCALE_H_
+
+#include <vector>
+#include <limits>
+
+#include "aos/vision/blob/range_image.h"
+
+namespace aos {
+namespace vision {
+
+// Bounding box for a RangeImage.
+struct ImageBBox {
+ int minx = std::numeric_limits<int>::max();
+ int maxx = std::numeric_limits<int>::min();
+ int miny = std::numeric_limits<int>::max();
+ int maxy = std::numeric_limits<int>::min();
+};
+
+// Sums img into bbox. bbox is constructed empty and grows with each call
+// to GetBBox.
+void GetBBox(const RangeImage &img, ImageBBox *bbox);
+inline void GetBBox(const std::vector<RangeImage> &imgs, ImageBBox *bbox) {
+ for (const auto &img : imgs) GetBBox(img, bbox);
+}
+
+std::vector<RangeImage> MoveScale(const std::vector<RangeImage> &imgs, int dx,
+ int dy, int scale);
+
+RangeImage MoveScale(const RangeImage &img, int dx, int dy, int scale);
+
+} // namespace vision
+} // namespace aos
+
+#endif // AOS_VISION_BLOB_MOVE_SCALE_H_
diff --git a/aos/vision/blob/range_image.h b/aos/vision/blob/range_image.h
index 109a100..3647890 100644
--- a/aos/vision/blob/range_image.h
+++ b/aos/vision/blob/range_image.h
@@ -53,6 +53,8 @@
int mini() const { return min_y_; }
+ int height() const { return min_y_ + ranges_.size(); }
+
private:
// minimum index in y where the blob starts
int min_y_ = 0;
diff --git a/aos/vision/blob/stream_view.h b/aos/vision/blob/stream_view.h
index bee1dc9..2ae56ac 100644
--- a/aos/vision/blob/stream_view.h
+++ b/aos/vision/blob/stream_view.h
@@ -61,6 +61,26 @@
}
}
+ inline void DrawSecondBlobList(const BlobList &blob_list, PixelRef color1,
+ PixelRef color2, PixelRef prev_color) {
+ ImagePtr ptr = img();
+ for (const auto &blob : blob_list) {
+ for (int i = 0; i < (int)blob.ranges().size(); ++i) {
+ for (const auto &range : blob.ranges()[i]) {
+ for (int j = range.st; j < range.ed; ++j) {
+ auto px = ptr.get_px(j, i + blob.min_y());
+ if (px.r == prev_color.r && px.g == prev_color.g &&
+ px.b == prev_color.b) {
+ ptr.get_px(j, i + blob.min_y()) = color2;
+ } else {
+ ptr.get_px(j, i + blob.min_y()) = color1;
+ }
+ }
+ }
+ }
+ }
+ }
+
// Backwards compatible.
DebugViewer *view() { return this; }
diff --git a/aos/vision/blob/test_utils.cc b/aos/vision/blob/test_utils.cc
new file mode 100644
index 0000000..7664f8a
--- /dev/null
+++ b/aos/vision/blob/test_utils.cc
@@ -0,0 +1,43 @@
+#include "aos/vision/blob/test_utils.h"
+
+namespace aos {
+namespace vision {
+
+RangeImage LoadFromTestData(int mini, const char *data) {
+ // Consume initial return.
+ if (*data) ++data;
+ std::vector<std::vector<ImageRange>> rows;
+ int x = 0;
+ bool p_score = false;
+ int pstart = -1;
+ std::vector<ImageRange> out_ranges;
+
+ for (; *data; ++data) {
+ char cell = *data;
+
+ if (cell == '\n') {
+ if (p_score) {
+ out_ranges.emplace_back(ImageRange(pstart, x));
+ }
+ rows.emplace_back(out_ranges);
+ out_ranges = {};
+ x = 0;
+ pstart = -1;
+ p_score = false;
+ } else {
+ if ((cell != ' ') != p_score) {
+ if (p_score) {
+ out_ranges.emplace_back(ImageRange(pstart, x));
+ } else {
+ pstart = x;
+ }
+ p_score = !p_score;
+ }
+ ++x;
+ }
+ }
+ return RangeImage(mini, std::move(rows));
+}
+
+} // namespace vision
+} // namespace aos
diff --git a/aos/vision/blob/test_utils.h b/aos/vision/blob/test_utils.h
new file mode 100644
index 0000000..e0c706c
--- /dev/null
+++ b/aos/vision/blob/test_utils.h
@@ -0,0 +1,24 @@
+#ifndef AOS_VISION_BLOB_TEST_UTILS_H_
+#define AOS_VISION_BLOB_TEST_UTILS_H_
+
+#include "aos/vision/blob/range_image.h"
+
+namespace aos {
+namespace vision {
+
+// For tests. Loads a RangeImage from a constant string.
+//
+// For example this should look something like this:
+// first and final returns will be stripped.
+//
+// R"(
+// --- ---
+// -----
+// --
+// )"
+RangeImage LoadFromTestData(int mini, const char *data);
+
+} // namespace vision
+} // namespace aos
+
+#endif // AOS_VISION_BLOB_TEST_UTILS_H_
diff --git a/aos/vision/blob/transpose.cc b/aos/vision/blob/transpose.cc
new file mode 100644
index 0000000..19ac965
--- /dev/null
+++ b/aos/vision/blob/transpose.cc
@@ -0,0 +1,87 @@
+#include "aos/vision/blob/transpose.h"
+
+#include <algorithm>
+
+namespace aos {
+namespace vision {
+
+RangeImage Transpose(const RangeImage &img) {
+ enum EventT {
+ // Must happen before point adds and deletes.
+ kRangeStart = 0,
+ kRangeEnd = 1,
+ // Non-overlapping
+ kPointAdd = 3,
+ kPointDel = 2,
+ };
+ std::vector<std::vector<std::pair<int, EventT>>> events;
+ int y = img.min_y();
+ for (const std::vector<ImageRange> &row : img) {
+ for (const ImageRange &range : row) {
+ if (range.ed >= static_cast<int>(events.size()))
+ events.resize(range.ed + 1);
+ events[range.st].emplace_back(y, kPointAdd);
+ events[range.ed].emplace_back(y, kPointDel);
+ }
+ ++y;
+ }
+
+ int min_y = 0;
+ while (min_y < (int)events.size() && events[min_y].empty()) ++min_y;
+
+ std::vector<ImageRange> prev_ranges;
+ std::vector<ImageRange> cur_ranges;
+
+ std::vector<std::vector<ImageRange>> rows;
+ for (int y = min_y; y < static_cast<int>(events.size()) - 1; ++y) {
+ auto row_events = std::move(events[y]);
+ for (const auto &range : prev_ranges) {
+ row_events.emplace_back(range.st, kRangeStart);
+ row_events.emplace_back(range.ed, kRangeEnd);
+ }
+ std::sort(row_events.begin(), row_events.end());
+ cur_ranges.clear();
+
+ bool has_cur_range = false;
+ ImageRange cur_range{0, 0};
+ auto add_range = [&](ImageRange range) {
+ if (range.st == range.ed) return;
+ if (has_cur_range) {
+ if (cur_range.ed == range.st) {
+ range = ImageRange{cur_range.st, range.ed};
+ } else {
+ cur_ranges.emplace_back(cur_range);
+ }
+ }
+ cur_range = range;
+ has_cur_range = true;
+ };
+
+ int prev_start;
+ for (const auto &pt : row_events) {
+ switch (pt.second) {
+ case kRangeStart:
+ prev_start = pt.first;
+ break;
+ case kPointAdd:
+ add_range(ImageRange{pt.first, pt.first + 1});
+ break;
+ case kRangeEnd:
+ add_range(ImageRange{prev_start, pt.first});
+ break;
+ case kPointDel:
+ add_range(ImageRange{prev_start, pt.first});
+ prev_start = pt.first + 1;
+ break;
+ }
+ }
+
+ if (has_cur_range) cur_ranges.emplace_back(cur_range);
+ rows.emplace_back(cur_ranges);
+ std::swap(cur_ranges, prev_ranges);
+ }
+ return RangeImage(min_y, std::move(rows));
+}
+
+} // namespace vision
+} // namespace aos
diff --git a/aos/vision/blob/transpose.h b/aos/vision/blob/transpose.h
new file mode 100644
index 0000000..632f270
--- /dev/null
+++ b/aos/vision/blob/transpose.h
@@ -0,0 +1,20 @@
+#ifndef _y2017_VISION_BLOB_TRANSPOSE_H_
+#define _y2017_VISION_BLOB_TRANSPOSE_H_
+
+#include "aos/vision/blob/range_image.h"
+
+namespace aos {
+namespace vision {
+
+RangeImage Transpose(const RangeImage &img);
+inline std::vector<RangeImage> Transpose(const std::vector<RangeImage> &imgs) {
+ std::vector<RangeImage> out;
+ out.reserve(imgs.size());
+ for (const auto &img : imgs) out.push_back(Transpose(img));
+ return out;
+}
+
+} // namespace vision
+} // namespace aos
+
+#endif // _y2017_VISION_ROT90_H_
diff --git a/aos/vision/blob/transpose_test.cc b/aos/vision/blob/transpose_test.cc
new file mode 100644
index 0000000..a5c2964
--- /dev/null
+++ b/aos/vision/blob/transpose_test.cc
@@ -0,0 +1,29 @@
+#include "aos/vision/blob/transpose.h"
+#include "aos/vision/blob/test_utils.h"
+
+#include <algorithm>
+#include <string>
+#include "gtest/gtest.h"
+
+namespace aos {
+namespace vision {
+
+TEST(TransposeTest, Tranpspose) {
+ RangeImage img = LoadFromTestData(20, R"(
+ -----------
+ ----- ----
+ ------------
+ -------------
+ ------------
+ ----------
+ ------------
+ ---------
+)");
+
+ auto b = Transpose(img);
+ auto c = Transpose(b);
+ EXPECT_EQ(ShortDebugPrint({img}), ShortDebugPrint({c}));
+}
+
+} // namespace vision
+} // namespace aos
diff --git a/frc971/constants.h b/frc971/constants.h
index d91e197..57637dd 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -19,8 +19,15 @@
};
struct EncoderPlusIndexZeroingConstants {
- // The amount of index pulses in the limb's range of motion.
- int num_index_pulses;
+ // The amount of index pulses in the joint's range of motion.
+ int index_pulse_count;
+ // The difference in scaled units between two index pulses.
+ double index_difference;
+ // The absolute position in scaled units of one of the index pulses.
+ double measured_index_position;
+ // The index pulse that is known, going from lowest in the range of motion to
+ // highest (Starting at 0).
+ int known_index_pulse;
};
struct PotAndAbsoluteEncoderZeroingConstants {
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index 171a1d1..6dd6f42 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -1,8 +1,9 @@
#include "frc971/zeroing/zeroing.h"
-#include <cmath>
-#include <vector>
#include <algorithm>
+#include <cmath>
+#include <limits>
+#include <vector>
#include "frc971/zeroing/wrap.h"
@@ -44,9 +45,10 @@
Reset();
}
+
void PotAndIndexPulseZeroingEstimator::Reset() {
samples_idx_ = 0;
- start_pos_ = 0;
+ offset_ = 0;
start_pos_samples_.clear();
zeroed_ = false;
wait_for_index_pulse_ = true;
@@ -111,12 +113,12 @@
// our best guess.
if (!zeroed_ &&
(info.index_pulses == last_used_index_pulse_count_ || !offset_ready())) {
- start_pos_ = start_average;
+ offset_ = start_average;
} 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.
- start_pos_ = CalculateStartPosition(start_average, info.latched_encoder);
+ 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
@@ -124,7 +126,7 @@
// Save the first starting position.
if (!zeroed_) {
- first_start_pos_ = start_pos_;
+ first_start_pos_ = offset_;
LOG(INFO, "latching start position %f\n", first_start_pos_);
}
@@ -133,20 +135,20 @@
zeroed_ = true;
// Throw an error if first_start_pos is bigger/smaller than
// constants_.allowable_encoder_error * index_diff + start_pos.
- if (::std::abs(first_start_pos_ - start_pos_) >
+ if (::std::abs(first_start_pos_ - offset_) >
constants_.allowable_encoder_error * constants_.index_difference) {
if (!error_) {
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_, start_pos_,
+ first_start_pos_, offset_,
constants_.allowable_encoder_error * constants_.index_difference);
error_ = true;
}
}
}
- position_ = start_pos_ + info.encoder;
+ position_ = offset_ + info.encoder;
filtered_position_ = start_average + info.encoder;
}
@@ -184,6 +186,13 @@
// update estimates based on those samples.
void PotAndAbsEncoderZeroingEstimator::UpdateEstimate(
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)) {
+ error_ = true;
+ return;
+ }
+
bool moving = true;
if (buffered_samples_.size() < constants_.moving_buffer_size) {
// Not enough samples to start determining if the robot is moving or not,
@@ -300,5 +309,63 @@
position_ = offset_ + info.encoder;
}
+void PulseIndexZeroingEstimator::Reset() {
+ max_index_position_ = ::std::numeric_limits<double>::lowest();
+ min_index_position_ = ::std::numeric_limits<double>::max();
+ offset_ = 0;
+ last_used_index_pulse_count_ = 0;
+ zeroed_ = false;
+ error_ = false;
+}
+
+void PulseIndexZeroingEstimator::StoreIndexPulseMaxAndMin(
+ const IndexPosition &info) {
+ // If we have a new index pulse.
+ 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 < min_index_position_) {
+ min_index_position_ = info.latched_encoder;
+ }
+ last_used_index_pulse_count_ = info.index_pulses;
+ }
+}
+
+int PulseIndexZeroingEstimator::IndexPulseCount() {
+ if (min_index_position_ > max_index_position_) {
+ // This condition means we haven't seen a pulse yet.
+ return 0;
+ }
+
+ // Calculate the number of pulses encountered so far.
+ return 1 + static_cast<int>(
+ ::std::round((max_index_position_ - min_index_position_) /
+ constants_.index_difference));
+}
+
+void PulseIndexZeroingEstimator::UpdateEstimate(const IndexPosition &info) {
+ StoreIndexPulseMaxAndMin(info);
+ const int index_pulse_count = IndexPulseCount();
+ if (index_pulse_count > constants_.index_pulse_count) {
+ error_ = true;
+ }
+
+ // TODO(austin): Detect if the encoder or index pulse is unplugged.
+ // TODO(austin): Detect missing counts.
+
+ if (index_pulse_count == constants_.index_pulse_count && !zeroed_) {
+ offset_ = constants_.measured_index_position -
+ constants_.known_index_pulse * constants_.index_difference -
+ min_index_position_;
+ zeroed_ = true;
+ }
+ if (zeroed_) {
+ position_ = info.encoder + offset_;
+ }
+}
+
} // namespace zeroing
} // namespace frc971
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index dc8a1ad..217a820 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -24,21 +24,16 @@
virtual ~ZeroingEstimator(){}
// Returns true if the logic considers the corresponding mechanism to be
- // zeroed. It return false otherwise. For example, right after a call to
- // Reset() this returns false.
+ // zeroed.
virtual bool zeroed() const = 0;
- // Returns the estimated starting position of the corresponding mechansim. In
- // some contexts we refer to this as the "offset".
+ // Returns the estimated position of the corresponding mechanism.
virtual double offset() const = 0;
- // Returns the estimated position of the corresponding mechanism. This value
- // is in SI units. For example, the estimator for the elevator would return a
- // value in meters for the height relative to absolute zero.
+ // Returns the estimated starting position of the corresponding mechansim.
virtual double position() const = 0;
- // Returns true if an error has occurred, false otherwise. This gets reset to
- // false when the Reset() function is called.
+ // Returns true if there has been an error.
virtual bool error() const = 0;
};
@@ -69,7 +64,7 @@
double position() const override { return position_; }
- double offset() const override { return start_pos_; }
+ double offset() const override { return offset_; }
// Return the estimated position of the corresponding mechanism not using the
// index pulse, even if one is available.
@@ -111,7 +106,7 @@
std::vector<double> start_pos_samples_;
// The estimated starting position of the mechanism. We also call this the
// 'offset' in some contexts.
- double start_pos_;
+ double offset_;
// Flag for triggering logic that takes note of the current index pulse count
// after a reset. See `last_used_index_pulse_count_'.
bool wait_for_index_pulse_;
@@ -155,8 +150,7 @@
double offset() const override { return offset_; }
- // TODO(austin): Actually implement this.
- bool error() const override { return false; }
+ bool error() const override { return error_; }
// Returns true if the sample buffer is full.
bool offset_ready() const {
@@ -195,6 +189,77 @@
double filtered_position_ = 0.0;
// The filtered position.
double position_ = 0.0;
+ // Whether or not there is an error in the estimate.
+ bool error_ = false;
+};
+
+
+// Zeros by seeing all the index pulses in the range of motion of the mechanism
+// and using that to figure out which index pulse is which.
+class PulseIndexZeroingEstimator : public ZeroingEstimator {
+ public:
+ using Position = IndexPosition;
+ using ZeroingConstants = constants::PotAndIndexPulseZeroingConstants;
+ using State = EstimatorState;
+
+ PulseIndexZeroingEstimator(
+ const constants::EncoderPlusIndexZeroingConstants &constants)
+ : constants_(constants) {
+ Reset();
+ }
+
+ // Resets the internal logic so it needs to be re-zeroed.
+ void Reset();
+
+ bool zeroed() const override { return zeroed_; }
+
+ double position() const override {
+ CHECK(zeroed_);
+ return position_;
+ }
+
+ double offset() const override { return offset_; }
+
+ bool error() const override { return error_; }
+
+ // Updates the internal logic with the next sensor values.
+ void UpdateEstimate(const IndexPosition &info);
+
+ private:
+ // Returns the current real position using the relative encoder offset.
+ double CalculateCurrentPosition(const IndexPosition &info);
+
+ // Sets the minimum and maximum index pulse position values.
+ void StoreIndexPulseMaxAndMin(const IndexPosition &info);
+
+ // Returns the number of index pulses we should have seen so far.
+ int IndexPulseCount();
+
+ // Contains the physical constants describing the system.
+ const constants::EncoderPlusIndexZeroingConstants constants_;
+
+ // The smallest position of all the index pulses.
+ double min_index_position_;
+ // The largest position of all the index pulses.
+ double max_index_position_;
+
+ // The estimated starting position of the mechanism.
+ double offset_;
+ // After a reset we keep track of the index pulse count with this. Only after
+ // the index pulse count changes (i.e. increments at least once or wraps
+ // around) will we consider the mechanism zeroed. We also use this to store
+ // the most recent `PotAndIndexPosition::index_pulses' value when the start
+ // position was calculated. It helps us calculate the start position only on
+ // index pulses to reject corrupted intermediate data.
+ uint32_t last_used_index_pulse_count_;
+
+ // True if we are fully zeroed.
+ bool zeroed_;
+ // Marker to track whether an error has occurred.
+ bool error_;
+
+ // The estimated position.
+ double position_;
};
// Populates an EstimatorState struct with information from the zeroing
diff --git a/frc971/zeroing/zeroing_test.cc b/frc971/zeroing/zeroing_test.cc
index 420ca61..6373056 100644
--- a/frc971/zeroing/zeroing_test.cc
+++ b/frc971/zeroing/zeroing_test.cc
@@ -18,6 +18,7 @@
using control_loops::PositionSensorSimulator;
using constants::PotAndIndexPulseZeroingConstants;
using constants::PotAndAbsoluteEncoderZeroingConstants;
+using constants::EncoderPlusIndexZeroingConstants;
static const size_t kSampleSize = 30;
static const double kAcceptableUnzeroedError = 0.2;
@@ -38,13 +39,21 @@
}
void MoveTo(PositionSensorSimulator *simulator,
- PotAndAbsEncoderZeroingEstimator *estimator, double new_position) {
+ PotAndAbsEncoderZeroingEstimator *estimator,
+ double new_position) {
PotAndAbsolutePosition sensor_values_;
simulator->MoveTo(new_position);
simulator->GetSensorValues(&sensor_values_);
estimator->UpdateEstimate(sensor_values_);
}
- // TODO(phil): Add new MoveTo overloads for different zeroing estimators.
+
+ void MoveTo(PositionSensorSimulator *simulator,
+ PulseIndexZeroingEstimator *estimator, double new_position) {
+ IndexPosition sensor_values_;
+ simulator->MoveTo(new_position);
+ simulator->GetSensorValues(&sensor_values_);
+ estimator->UpdateEstimate(sensor_values_);
+ }
::aos::testing::TestSharedMemory my_shm_;
};
@@ -365,5 +374,76 @@
MoveTo(&sim, &estimator, start_pos);
ASSERT_FALSE(estimator.zeroed());
}
+
+// Makes sure we detect an error if the ZeroingEstimator gets sent a NaN.
+TEST_F(ZeroingTest, TestAbsoluteEncoderZeroingWithNaN) {
+ PotAndAbsoluteEncoderZeroingConstants constants{
+ kSampleSize, 1, 0.3, 0.1, kMovingBufferSize};
+
+ PotAndAbsEncoderZeroingEstimator 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;
+ estimator.UpdateEstimate(sensor_values_);
+
+ ASSERT_TRUE(estimator.error());
+}
+
+// Makes sure that using only a relative encoder with index pulses allows us to
+// successfully zero.
+// We pretend that there are index pulses at 10, 20, and 30.
+TEST_F(ZeroingTest, TestRelativeEncoderZeroing) {
+ EncoderPlusIndexZeroingConstants constants;
+ constants.index_pulse_count = 3;
+ constants.index_difference = 10.0;
+ constants.measured_index_position = 20.0;
+ constants.known_index_pulse = 1;
+
+ PositionSensorSimulator sim(constants.index_difference);
+
+ const double start_pos = 2.5 * constants.index_difference;
+
+ sim.Initialize(start_pos, constants.index_difference / 3.0,
+ constants.measured_index_position);
+
+ PulseIndexZeroingEstimator estimator(constants);
+
+ // Should not be zeroed when we stand still.
+ for (int i = 0; i < 300; ++i) {
+ MoveTo(&sim, &estimator, start_pos);
+ ASSERT_FALSE(estimator.zeroed());
+ }
+
+ // Move to 1.5 constants.index_difference and we should still not be zeroed.
+ MoveTo(&sim, &estimator, 1.5 * constants.index_difference);
+ ASSERT_FALSE(estimator.zeroed());
+
+ // Move to 0.5 constants.index_difference and we should still not be zeroed.
+ MoveTo(&sim, &estimator, 0.5 * constants.index_difference);
+ ASSERT_FALSE(estimator.zeroed());
+
+ // Move back to 1.5 constants.index_difference and we should still not be
+ // zeroed.
+ MoveTo(&sim, &estimator, 1.5 * constants.index_difference);
+ ASSERT_FALSE(estimator.zeroed());
+
+ // Move back to 2.5 constants.index_difference and we should still not be
+ // zeroed.
+ MoveTo(&sim, &estimator, 2.5 * constants.index_difference);
+ ASSERT_FALSE(estimator.zeroed());
+
+ // Move back to 3.5 constants.index_difference and we should now be zeroed.
+ MoveTo(&sim, &estimator, 3.5 * constants.index_difference);
+ ASSERT_TRUE(estimator.zeroed());
+
+ ASSERT_DOUBLE_EQ(start_pos, estimator.offset());
+ ASSERT_DOUBLE_EQ(3.5 * constants.index_difference, estimator.position());
+
+ MoveTo(&sim, &estimator, 0.5 * constants.index_difference);
+ ASSERT_DOUBLE_EQ(0.5 * constants.index_difference, estimator.position());
+}
+
} // namespace zeroing
} // namespace frc971
diff --git a/tools/BUILD b/tools/BUILD
index 59cf695..8926d08 100644
--- a/tools/BUILD
+++ b/tools/BUILD
@@ -25,13 +25,13 @@
)
config_setting(
name = 'has_asan',
- values = {'copt': '-fsanitize=address'},
+ values = {'define': 'have_asan=true'},
)
config_setting(
name = 'has_tsan',
- values = {'copt': '-fsanitize=thread'},
+ values = {'define': 'have_tsan=true'},
)
config_setting(
name = 'has_ubsan',
- values = {'copt': '-fsanitize=undefined'},
+ values = {'define': 'have_ubsan=true'},
)
diff --git a/tools/bazel.rc b/tools/bazel.rc
index 7530102..c8d93d0 100644
--- a/tools/bazel.rc
+++ b/tools/bazel.rc
@@ -15,16 +15,19 @@
build:asan --test_env ASAN_SYMBOLIZER_PATH=/usr/bin/llvm-symbolizer-3.6
build:asan --test_env ASAN_OPTIONS=detect_leaks=1:check_initialization_order=1:strict_init_order=1:detect_stack_use_after_return=1:detect_odr_violation=2:allow_user_segv_handler=1
build:asan --copt -fsanitize-blacklist=tools/cpp/asan-blacklist
+build:asan --define have_asan=true
build:tsan --copt -fsanitize=thread --copt -DAOS_SANITIZER_thread
build:tsan --linkopt -fsanitize=thread
build:tsan --platform_suffix=-tsan
build:tsan --test_env TSAN_OPTIONS=external_symbolizer_path=/usr/bin/llvm-symbolizer-3.6:detect_deadlocks=1:second_deadlock_stack=1
+build:tsan --define have_tsan=true
build:isan --copt -fsanitize=integer
build:isan --linkopt -fsanitize=integer
build:isan --platform_suffix=-isan
build:isan --test_env LLVM_SYMBOLIZER=/usr/bin/llvm-symbolizer-3.6
+build:isan --define have_isan=true
build:ubsan --copt -fsanitize=undefined --copt -fno-sanitize-recover
# Bad alignment is just slow on x86 and traps on ARM, so we'll find
@@ -36,11 +39,13 @@
build:ubsan --platform_suffix=-ubsan
build:ubsan --test_env UBSAN_OPTIONS=external_symbolizer_path=/usr/bin/llvm-symbolizer-3.6:color=always:print_stacktrace=1
build:ubsan --copt -fsanitize-blacklist=tools/cpp/ubsan-blacklist
+build:ubsan --define have_ubsan=true
build:msan --copt -fsanitize=memory --copt -fsanitize-memory-track-origins
build:msan --linkopt -fsanitize=memory --linkopt -fsanitize-memory-track-origins
build:msan --platform_suffix=-msan
build:msan --test_env MSAN_SYMBOLIZER_PATH=/usr/bin/llvm-symbolizer-3.6
+build:msan --define have_msan=true
# Show paths to a few more than just 1 target.
build --show_result 5
diff --git a/y2017/control_loops/superstructure/indexer/indexer.cc b/y2017/control_loops/superstructure/indexer/indexer.cc
index cab9864..78e9528 100644
--- a/y2017/control_loops/superstructure/indexer/indexer.cc
+++ b/y2017/control_loops/superstructure/indexer/indexer.cc
@@ -177,12 +177,12 @@
monotonic_now > last_transition_time_ + kReverseMinTimeout) ||
monotonic_now > kReverseTimeout + last_transition_time_) {
state_ = State::RUNNING;
- last_transition_time_ = monotonic_now;
// Only reset if we got stuck going this way too.
if (monotonic_now > kReverseTimeout + last_transition_time_) {
indexer_.Reset();
}
+ last_transition_time_ = monotonic_now;
}
break;
}