Finished logic and fixed errors for index pulse zeroing
Changed some small logical errors and fixed compiler errors to finish
the index pulse zeroing system (for use without a potentiometer.
Error checking still needs to be added in the future.
Change-Id: I5f7b06d5d6de9eebcc918256632f017ef2b0736b
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 22e3b3d..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;
}
@@ -307,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 d850f54..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_;
@@ -198,6 +193,75 @@
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
// estimator.
void PopulateEstimatorState(const PotAndIndexPulseZeroingEstimator &estimator,
diff --git a/frc971/zeroing/zeroing_test.cc b/frc971/zeroing/zeroing_test.cc
index 526a514..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;
@@ -45,7 +46,14 @@
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_;
};
@@ -383,5 +391,59 @@
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