keitaro takeuchi / omniwheel

Omni-wheel robot wheel speed calculation library

omni.h@2:e4b06a9b3ff2, 2022-06-20 (annotated)

Committer:
Keitaro Takeuchi
Date:
Mon Jun 20 21:28:05 2022 +0900
Revision:
2:e4b06a9b3ff2
Parent:
1:74cfff9b981e 
tiny fix

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Keitaro Takeuchi 0:14717ae9875d 1 #include <array>
Keitaro Takeuchi 0:14717ae9875d 2 #include <cmath>
Keitaro Takeuchi 1:74cfff9b981e 3 #include <algorithm>
Keitaro Takeuchi 0:14717ae9875d 4
Keitaro Takeuchi 0:14717ae9875d 5 template <size_t wheelSize>
Keitaro Takeuchi 0:14717ae9875d 6 class Omni {
Keitaro Takeuchi 0:14717ae9875d 7 public:
Keitaro Takeuchi 2:e4b06a9b3ff2 8 Omni(const std::array<float, wheelSize>& radians) : radians(radians) {}
Keitaro Takeuchi 2:e4b06a9b3ff2 9
Keitaro Takeuchi 2:e4b06a9b3ff2 10 void computeXY(const float& x, const float& y, const float& a) {
Keitaro Takeuchi 2:e4b06a9b3ff2 11 computeCircular(std::hypot(x, y), std::atan2(x, y), a);
Keitaro Takeuchi 0:14717ae9875d 12 }
Keitaro Takeuchi 0:14717ae9875d 13
Keitaro Takeuchi 2:e4b06a9b3ff2 14 void computeCircular(const float& r, const float& rad, const float& a) {
Keitaro Takeuchi 2:e4b06a9b3ff2 15 for(size_t i = 0; i < wheelSize; i++) {
Keitaro Takeuchi 0:14717ae9875d 16 m_outs[i] = sin(rad + radians[i]) * r + a;
Keitaro Takeuchi 0:14717ae9875d 17 }
Keitaro Takeuchi 1:74cfff9b981e 18 const float maxOut = *std::max_element(m_outs.begin(), m_outs.end(), [](const auto& a, const auto& b){return std::abs(a) < std::abs(b);});
Keitaro Takeuchi 0:14717ae9875d 19 if(maxOut > 1.0) {
Keitaro Takeuchi 0:14717ae9875d 20 for(auto& out: m_outs) {
Keitaro Takeuchi 0:14717ae9875d 21 out *= (1.0 / maxOut);
Keitaro Takeuchi 0:14717ae9875d 22 }
Keitaro Takeuchi 0:14717ae9875d 23 }
Keitaro Takeuchi 0:14717ae9875d 24 }
Keitaro Takeuchi 0:14717ae9875d 25
Keitaro Takeuchi 0:14717ae9875d 26 const std::array<float, wheelSize> outs() { return m_outs; }
Keitaro Takeuchi 0:14717ae9875d 27
Keitaro Takeuchi 0:14717ae9875d 28 typename std::array<float, wheelSize>::iterator begin() { return m_outs.begin(); }
Keitaro Takeuchi 0:14717ae9875d 29 typename std::array<float, wheelSize>::iterator end() { return m_outs.end(); }
Keitaro Takeuchi 0:14717ae9875d 30
Keitaro Takeuchi 0:14717ae9875d 31 private:
Keitaro Takeuchi 0:14717ae9875d 32 std::array<float, wheelSize> radians;
Keitaro Takeuchi 0:14717ae9875d 33 std::array<float, wheelSize> m_outs;
Keitaro Takeuchi 0:14717ae9875d 34 };