omni wheel library
Dependents: quadOmni_yanagi NHK2017_octopus hayatoShooter
オムニ用のライブラリです。
Diff: omni.cpp
- Revision:
- 9:a30169882a15
- Parent:
- 0:979565e955a5
diff -r 979565e955a5 -r a30169882a15 omni.cpp --- a/omni.cpp Mon Jul 03 14:23:36 2017 +0000 +++ b/omni.cpp Wed Aug 23 07:24:34 2017 +0000 @@ -1,5 +1,13 @@ #include "omni.h" +Omni::Omni(int wheels) : wheels(wheels) +{ + for(int i = 0; i < wheels; i++) { + wheel[i] = 0; + radian[i] = 0; + } +} + inline double fmax(double a, double b) { if(a > b) { @@ -18,83 +26,87 @@ } } -inline double toRadians(double degrees) { - return degrees * (M_PI / 180.0); +bool isInRange(float X, float Y) +{ + if(X < -1 || X > 1 || Y < -1 || Y > 1) return 0; + if(hypot(X, Y) > 1.0) return 0; + return 1; +} + +bool isInRange(float r) +{ + if(r < 0 || r > 1) return 0; + return 1; +} + + +bool Omni::setWheelRadian(float rad1, float rad2, float rad3) +{ + if(wheels != 3) return 0; + radian[0] = rad1; + radian[1] = rad2; + radian[2] = rad3; + return 1; } -Omni::Omni(int wheels, double initDegree) : - wheels(wheels), initDegree(initDegree) +bool Omni::setWheelRadian(float rad1, float rad2, float rad3, float rad4) +{ + if(wheels != 4) return 0; + radian[0] = rad1; + radian[1] = rad2; + radian[2] = rad3; + radian[3] = rad4; + return 1; +} + +bool Omni::setWheelRadian(int wheelNumber, float rad) { - for(int i = 0; i < wheels; i++) { - wheel[i] = 0; + if(wheelNumber > 3|| wheelNumber <= 0) return 0; + radian[wheelNumber] = rad; + return 1; +} + +bool Omni::computeXY(float X, float Y, float moment) +{ + if(isInRange(X, Y)) { + return computeCircular(hypot(X, Y), atan2(Y, X), moment); + } else { + return 0; } } -bool Omni::computeXY(double parallelVector[], double moment) + +bool Omni::computeCircular(float r, float rad, float 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]); + if(wheels <= 0) return 0; + if(isInRange(r)) { + double parallel[4] = {0}; + double parallelMax = -1; + double parallelMin = 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) { + for(int i = 0; i < wheels; i++) { + parallel[i] = sin(rad + radian[i]) * r; + } + 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; + } else { 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) +float Omni::getOutput(int wheelNumber) { - wheels = wheel; -} - -void Omni::setInitDegree(double degree) -{ - initDegree = degree; -} - -double Omni::getOutput(int num) -{ - return wheel[num]; -} + return wheel[wheelNumber]; +} \ No newline at end of file