frc971/zeroing/zeroing.cc

#include "frc971/zeroing/zeroing.h"

#include <math.h>
#include <vector>

namespace frc971 {
namespace zeroing {

void PopulateEstimatorState(const zeroing::ZeroingEstimator &estimator,
                            EstimatorState *state) {
  state->error = estimator.error();
  state->zeroed = estimator.zeroed();
  state->position = estimator.position();
}

ZeroingEstimator::ZeroingEstimator(
    const constants::Values::ZeroingConstants& constants) {
  index_diff_ = constants.index_difference;
  max_sample_count_ = constants.average_filter_size;
  known_index_pos_ = constants.measured_index_position;

  start_pos_samples_.reserve(max_sample_count_);

  Reset();
}

void ZeroingEstimator::Reset() {
  samples_idx_ = 0;
  start_pos_ = 0;
  start_pos_samples_.clear();
  zeroed_ = false;
  wait_for_index_pulse_ = true;
  last_used_index_pulse_count_ = 0;
  error_ = false;
}

void ZeroingEstimator::TriggerError() {
  if (!error_) {
    LOG(ERROR, "Manually triggered zeroing error.\n");
    error_ = true;
  }
}

double ZeroingEstimator::CalculateStartPosition(double start_average,
                                                double latched_encoder) const {
  // We calculate an aproximation of the value of the last index position.
  // Also account for index pulses not lining up with integer multiples of the
  // index_diff.
  double index_pos = start_average + latched_encoder - known_index_pos_;
  // We round index_pos to the closest valid value of the index.
  double accurate_index_pos = (round(index_pos / index_diff_)) * index_diff_;
  // Now we reverse the first calculation to get the accurate start position.
  return accurate_index_pos - latched_encoder + known_index_pos_;
}

void ZeroingEstimator::UpdateEstimate(const PotAndIndexPosition& info) {
  // We want to make sure that we encounter at least one index pulse while
  // zeroing. So we take the index pulse count from the first sample after
  // 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;
    wait_for_index_pulse_ = false;
  }

  if (start_pos_samples_.size() < max_sample_count_) {
    start_pos_samples_.push_back(info.pot - info.encoder);
  } else {
    start_pos_samples_[samples_idx_] = info.pot - info.encoder;
  }

  // Drop the oldest sample when we run this function the next time around.
  samples_idx_ = (samples_idx_ + 1) % max_sample_count_;

  double sample_sum = 0.0;

  for (size_t i = 0; i < start_pos_samples_.size(); ++i) {
    sample_sum += start_pos_samples_[i];
  }

  // Calculates the average of the starting position.
  double start_average = sample_sum / start_pos_samples_.size();

  // If there are no index pulses 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 (!zeroed_ && (info.index_pulses == last_used_index_pulse_count_ ||
                   offset_ratio_ready() < 1.0)) {
    start_pos_ = 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);
    last_used_index_pulse_count_ = info.index_pulses;

    // Now that we have an accurate starting position we can consider ourselves
    // zeroed.
    zeroed_ = true;
  }

  pos_ = start_pos_ + info.encoder;
}

}  // namespace zeroing
}  // namespace frc971