motion.cc

#include "motion.h"

#include <cmath>
#include <iostream>

using namespace std;

/**
 * @param distance in encoder ticks
 * @param max_v in ticks/100ms
 * @param max_v_time time to max velocity in milliseconds
 * @param max_a_time time to max acceleration in milliseconds
 * @param period in milliseconds
 */
Motion::Motion(unsigned distance,
               unsigned max_v,
               unsigned max_v_time,
               unsigned max_a_time,
               unsigned period)
    : distance_(distance),
      t1_(max_v_time),
      t2_(max_a_time),
      v_prog_(max_v / 100.0),
      f1_len_(static_cast<unsigned>(ceil(t1_ / period))),
      f2_len_(static_cast<unsigned>(ceil(t2_ / period))),
      period_(period) {
  double t4 = distance_ / v_prog_;  // ms
  total_inputs_ = static_cast<unsigned>(ceil(t4 / period));
}

void Motion::Run() {
  int input = iteration_++ < total_inputs_ ? 1 : 0;
  f1_.push(input);
  f1_total_ += input;
  if (f1_.size() > f1_len_) {
    f1_total_ -= f1_.front();
    f1_.pop();
  }
  double f1_output = f1_total_ / f1_len_;

  f2_.push(f1_output);
  f2_total_ += f1_output;
  if (f2_.size() > f2_len_) {
    f2_total_ -= f2_.front();
    f2_.pop();
  }

  curr_vel_ =
      (f1_total_ + f1_output + f2_total_) / (f1_len_ + f2_len_ + 1) * v_prog_;
  elapsed_distance_ += ((prev_vel_ + curr_vel_) / 2.0) * period_;
  accel_ = (curr_vel_ - prev_vel_) / period_;
  prev_vel_ = curr_vel_;
  elapsed_time_ += period_;
}

void Motion::OutputCSV() {
  cout << elapsed_time_ << "," << curr_vel_ * 100 << "," << elapsed_distance_
       << "," << accel_ * 100 * 100 << endl;
}

void Motion::DoIt() {
  size_t count = total_inputs_ + f1_len_ + f2_len_;
  cout << "Time (ms),Velocity,Distance,Acceleration\n0.0,0.0,0.0,0.0" << endl;
  for (size_t i = 0; i < count; i++) {
    Run();
    OutputCSV();
  }
}
// f1 is boxcar filter