omni wheel library
Dependents: quadOmni_yanagi NHK2017_octopus hayatoShooter
オムニ用のライブラリです。
Diff: omni.cpp
- Revision:
- 0:979565e955a5
- Child:
- 5:f2fc4a0b6a6f
- Child:
- 9:a30169882a15
diff -r 000000000000 -r 979565e955a5 omni.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/omni.cpp Mon Jul 03 14:23:36 2017 +0000 @@ -0,0 +1,100 @@ +#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); +} + +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] = 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 = 0; + double parallelMin = 0; + + for(int i = 0; i < wheels; i++) { + parallel[i] = sin(toRadians(parallelVector[0] + initDegree)) * parallelVector[1]; + } + for(int i = 0; i < wheels - 1; i++) { + parallelMax = fmax(parallel[i], parallel[i + 1]); + parallelMin = fmin(parallel[i], parallel[i + 1]); + } + 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]; +}