SteerDrive.h

Go to the documentation of this file.

#ifndef CHASSIS_STEER_DRIVE_H_
#define CHASSIS_STEER_DRIVE_H_
#include <CoordinateUnit.h>
 
#include <array>
#include <cmath>
#include <complex>
#include <functional>
#include <utility>
 
namespace rct {
 
 
template<int N>
struct SteerDrive {
  static_assert(N > 0, "template parameter N must be greater than 0");
 
  template<class F>
  SteerDrive(F&& f) : f_{std::forward<F>(f)} {}
 
  static constexpr int size() noexcept {
    return N;
  }
 
  void move(const Velocity& vel, float offset_rad = 0.0) {
    std::array<std::complex<float>, N> vels;
    const std::complex<float> rotate = std::polar<float>(vel.ang_rad, 0);
    for(int i = 0; i < N; ++i) {
      float ang = -offset_rad - M_PI / N * (2 * i + 1) + std::atan2(vel.y_milli, vel.x_milli);
      const std::complex<float> vel_fix = std::polar<float>(std::hypot(vel.x_milli, vel.y_milli), ang);
      vels[i] = vel_fix + rotate;
    }
    f_(std::move(vels));
  }
 private:
  std::function<void(std::array<std::complex<float>, N>)> f_;
};
 
 
}  // namespace rct
 
#endif  // CHASSIS_STEER_DRIVE_H_