omni wheel library
Dependents: quadOmni_yanagi NHK2017_octopus hayatoShooter
オムニ用のライブラリです。
omni.cpp
- Committer:
- UCHITAKE
- Date:
- 2017-08-06
- Revision:
- 7:1d0aa4275eb0
- Parent:
- 6:59171f3235b6
- Child:
- 8:ab46e745cfab
File content as of revision 7:1d0aa4275eb0:
#include "omni.h" inline double fmax(double a, double b) { if(a > b) { return a; } else { return b; } } inline double fmin(double a, double b) { if(a < b) { return a; } else { return b; } } inline double toRadians(double degrees) { return degrees * (M_PI / 180.0); } inline double toDegrees(double radians) { return radianss * (180.0 / M_PI); } Omni::Omni(int wheels, double initDegree) : wheels(wheels), initDegree(initDegree) { for(int i = 0; i < wheels; i++) { wheel[i] = 0; } } bool Omni::computeXY(double parallelVector[], double moment) { if(wheels < 3 || wheels > 4) { return 0; } double polar[2]; polar[0] = toDegrees(atan2(parallelVector[1], parallelVector[0])); polar[1] = hypot(parallelVector[0], parallelVector[1]); return computePolar(polar, moment); } bool Omni::computePolar(double parallelVector[], double moment) { if(wheels < 3 || wheels > 4) { return 0; } if(parallelVector[1] < 0 || parallelVector[1] > 1.0) { return 0; } double parallel[4] = {0}; double parallelMax = -1; double parallelMin = 1; for(int i = 0; i < wheels; i++) { parallel[i] = sin(toRadians(parallelVector[0] + initDegree + (360 / wheels) * i)) * parallelVector[1]; } for(int i = 0; i < wheels; i++) { parallelMax = fmax(parallel[i], parallelMax); parallelMin = fmin(parallel[i], parallelMin); } if(parallelMax + moment > 1.0 || parallelMin + moment < -1.0) { for(int i = 0; i < wheels; i++) { parallel[i] *= (1.0 - fabs(moment)); } } for(int i = 0; i < wheels; i++) { wheel[i] = parallel[i] + moment; } return 1; } bool Omni::stop() { for(int i = 0; i < wheels; i++) { wheel[i] = 0; } return 1; } void Omni::setWheels(int wheel) { wheels = wheel; } void Omni::setInitDegree(double degree) { initDegree = degree; } double Omni::getOutput(int num) { return wheel[num]; }