Odom.h

Go to the documentation of this file.

#ifndef CHASSIS_ODOM_H_
#define CHASSIS_ODOM_H_
#include <CoordinateUnit.h>
 
#include <cmath>
#include <utility>
 
namespace rct {
 
 
template<int N>
struct Odom {
  static_assert(N > 0, "template parameter N must be greater than 0");
 
  Odom(const Coordinate& pos = {}) : pos_{pos} {}
  static constexpr int size() noexcept {
    return N;
  }
 
  void integrate(const int (&dif_val)[N]) {
    constexpr float pi_N = M_PI / N;
    const float tmp_rad = pos_.ang_rad;
    for(int i = 0; i < N; ++i) {
      pos_.x_milli += dif_val[i] * cos((2 * i + 1) * pi_N + tmp_rad);
      pos_.y_milli += dif_val[i] * sin((2 * i + 1) * pi_N + tmp_rad);
      pos_.ang_rad += dif_val[i];
    }
  }
 
  const Coordinate& get() const& noexcept {
    return pos_;
  }
  Coordinate get() && noexcept {
    return std::move(pos_);
  }
 
  void set(const Coordinate& pos) noexcept {
    pos_ = pos;
  }
 
 private:
  Coordinate pos_;
};
 
 
}  // namespace rct
 
#endif  // CHASSIS_ODOM_H_