Added a start at a fridge.
Change-Id: I5bab9686f966368161ede528ff7abf038d7bcb9a
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index a0d3ba4..f6eddb5 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -5,14 +5,33 @@
namespace frc971 {
namespace zeroing {
+ZeroingEstimator::ZeroingEstimator(
+ const constants::Values::ZeroingConstants& constants) {
+ DoInit(constants.index_difference, constants.average_filter_size);
+}
+
ZeroingEstimator::ZeroingEstimator(double index_difference,
size_t max_sample_count) {
+ DoInit(index_difference, max_sample_count);
+}
+
+void ZeroingEstimator::DoInit(double index_difference,
+ size_t max_sample_count) {
index_diff_ = index_difference;
samples_idx_ = 0;
max_sample_count_ = max_sample_count;
start_pos_samples_.reserve(max_sample_count);
}
+void ZeroingEstimator::UpdateEstimate(const PotAndIndexPosition& info) {
+ ZeroingInfo zinfo;
+ zinfo.pot = info.pot;
+ zinfo.encoder = info.encoder;
+ zinfo.index_encoder = info.latched_encoder;
+ zinfo.index_count = info.index_pulses;
+ UpdateEstimate(zinfo);
+}
+
void ZeroingEstimator::UpdateEstimate(const ZeroingInfo & info) {
if (start_pos_samples_.size() < max_sample_count_) {
start_pos_samples_.push_back(info.pot - info.encoder);
@@ -32,19 +51,21 @@
* the average of the starting position to
* calculate the position.
*/
+ double pos;
if (info.index_count == 0) {
- pos_ = start_average + info.encoder;
+ pos = start_average + info.encoder;
+ zeroed_ = false;
} else {
- // We calculate an aproximation of the value of the last index position.
+ // We calculate an aproximation of the value of the last index position.
double index_pos = start_average + info.index_encoder;
- // We round index_pos to the closest valid value of the index.
+ // We round index_pos to the closest valid value of the index.
double accurate_index_pos = (round(index_pos / index_diff_)) * index_diff_;
- // We use accurate_index_pos to calculate the position.
- pos_ = accurate_index_pos + info.encoder - info.index_encoder;
+ // We use accurate_index_pos to calculate the position.
+ pos = accurate_index_pos + info.encoder - info.index_encoder;
+ zeroed_ = true;
}
+ offset_ = pos - info.encoder;
}
-double ZeroingEstimator::getPosition() { return pos_; }
-
} // namespace zeroing
} // namespace frc971
diff --git a/frc971/zeroing/zeroing.gyp b/frc971/zeroing/zeroing.gyp
index 973ca9c..3d17f13 100644
--- a/frc971/zeroing/zeroing.gyp
+++ b/frc971/zeroing/zeroing.gyp
@@ -19,6 +19,13 @@
],
'dependencies': [
'zeroing_queue',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:queues',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ ],
+ 'export_dependent_settings': [
+ 'zeroing_queue',
+ '<(DEPTH)/frc971/frc971.gyp:constants',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:queues',
],
},
{
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index 1e3bac9..89ee9da 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -3,6 +3,8 @@
#include <vector>
#include "frc971/zeroing/zeroing_queue.q.h"
+#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/constants.h"
namespace frc971 {
namespace zeroing {
@@ -12,11 +14,21 @@
class ZeroingEstimator {
public:
ZeroingEstimator(double index_difference, size_t max_sample_count);
- void UpdateEstimate(const ZeroingInfo& info);
- double getPosition();
+ ZeroingEstimator(const constants::Values::ZeroingConstants &constants);
+ void UpdateEstimate(const PotAndIndexPosition &info);
+ void UpdateEstimate(const ZeroingInfo &info);
+
+ double offset() const { return offset_; }
+ bool zeroed() const { return zeroed_; }
+ double offset_ratio_ready() const {
+ return start_pos_samples_.size() / static_cast<double>(max_sample_count_);
+ }
+
private:
- // The estimated position.
- double pos_;
+ void DoInit(double index_difference, size_t max_sample_count);
+
+ double offset_ = 0.0;
+ bool zeroed_ = false;
// The distance between two consecutive index positions.
double index_diff_;
// The next position in 'start_pos_samples_' to be used to store the
diff --git a/frc971/zeroing/zeroing_test.cc b/frc971/zeroing/zeroing_test.cc
index 342bc0b..ce636b6 100644
--- a/frc971/zeroing/zeroing_test.cc
+++ b/frc971/zeroing/zeroing_test.cc
@@ -95,19 +95,18 @@
}
ZeroingInfo getInfo() {
- ZeroingInfo estimate;
- estimate.pot = noise_generator_.AddNoiseToSample(cur_pos_);
+ estimate_.pot = noise_generator_.AddNoiseToSample(cur_pos_);
if (index_count_ == 0) {
- estimate.index_encoder = 0.0;
+ estimate_.index_encoder = 0.0;
} else {
- estimate.index_encoder = cur_index_ * index_diff_ - start_pos_;
+ estimate_.index_encoder = cur_index_ * index_diff_ - start_pos_;
}
- estimate.index_count = index_count_;
- estimate.encoder = cur_pos_ - start_pos_ - encoder_slip_;
- return estimate;
+ estimate_.index_count = index_count_;
+ estimate_.encoder = cur_pos_ - start_pos_ - encoder_slip_;
+ return estimate_;
}
- double getEstimate(void) { return estimator_.getPosition(); }
+ double getEstimate(void) { return estimator_.offset() + estimate_.encoder; }
private:
int index_count_;
@@ -119,6 +118,7 @@
double encoder_slip_;
ZeroingEstimator estimator_;
NoiseGenerator& noise_generator_;
+ ZeroingInfo estimate_;
};
class QueueTest : public ::testing::Test {