omni wheel library
Dependents: quadOmni_yanagi NHK2017_octopus hayatoShooter
オムニ用のライブラリです。
Revision 10:4f0b55344c73, committed 2017-08-23
- Comitter:
- UCHITAKE
- Date:
- Wed Aug 23 07:26:26 2017 +0000
- Parent:
- 8:ab46e745cfab
- Commit message:
- fix commit
Changed in this revision
omni.cpp | Show annotated file Show diff for this revision Revisions of this file |
omni.h | Show annotated file Show diff for this revision Revisions of this file |
--- a/omni.cpp Sun Aug 06 14:49:01 2017 +0000 +++ b/omni.cpp Wed Aug 23 07:26:26 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,87 +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; } -inline double toDegrees(double radians) { - return radians * (180.0 / M_PI); +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; } -Omni::Omni(int wheels, double initDegree) : - wheels(wheels), initDegree(initDegree) +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] = toDegrees(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 = -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) +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
--- a/omni.h Sun Aug 06 14:49:01 2017 +0000 +++ b/omni.h Wed Aug 23 07:26:26 2017 +0000 @@ -1,90 +1,80 @@ -/** - * @author keitaro takeuchi - * - * @section DESCRIPTION - * 3or4wheels omni libraryfor NHK2017. - */ - -#ifndef OMNI -#define OMNI - -/** - * Includes - */ +/** + * @file omni.h + * @author keitaro takeuchi + * + * @section DESCRIPTION + * 3or4wheels omni libraryfor NHK2017. + */ +#ifndef OMNI_H +#define OMNI_H + +/** + * Includes + */ #include "mbed.h" - -/** - * Defines - */ + +/** + * Defines + */ #define M_PI 3.141592653589793 - -/** - * omni wheel - */ + +/** + * omni wheel + */ class Omni { -public: - - /** - * Constructor. - * - * @param wheels 車輪数(3or4) - * @param initDegree 正面から車輪の角度 - */ - Omni(int wheels, double initDegree); - - /** - * 位置ベクトル(x, y), 回転量から出力を計算 - * - * @param parallelVector[] [x, y](-1 ~ 1) - * @param moment 回転量(-1 ~ 1) - * - * @return 1...success 0...failure - */ - bool computeXY(double parallelVector[], double moment); - - /** - * 角度, 大きさ(θ, volume), 回転量から出力を計算 - * - * @param parallelVector[] [θ(degree), volume] volume...(-1 ~ 1) - * @param moment 回転量(-1 ~ 1) - * @return 1...success 0...failure - */ - bool computePolar(double parallelVector[], double moment); - - /** - * 全ての出力を0にする - * - * @return 1...success 0...failure - */ - bool stop(); - - /** - * ホイール数を設定 - * - * @param wheel ホイール数 - */ - void setWheels(int wheel); - - /** - * 正面から車輪の角度を設定 - * - * @param degree 正面から車輪の角度 - */ - void setInitDegree(double degree); - - /** - * 出力値を取得 - * - * @param wheel wheel番目のホイールの出力を設定 - * - * @return 出力値 - */ - double getOutput(int wheel); +public : -private: + /** + * Constructor. + * + * @param wheels 車輪数(3or4) + */ + Omni(int wheels); + + /** + * ホイールの付いている角度を設定 + * + * @param rad radian + * @param wheelNumber 番目のホイール + */ + bool setWheelRadian(float rad1, float rad2, float rad3); + bool setWheelRadian(float rad1, float rad2, float rad3, float rad4); + bool setWheelRadian(int wheelNumber, float rad); + + /** + * 位置ベクトル(x, y), 回転量から出力を計算 + * + * @param X(-1 ~ 1) + * @param Y(-1 ~ 1) + * @param moment 回転量(-1 ~ 1) + * + * @return 1...success 0...failure + */ + bool computeXY(float X, float Y, float moment); + + /** + * 半径, 角度radian(r, Θ), 回転量から出力を計算 + * + * @param r 半径(0 ~ 1) + * @param rad radian(-PI ~ PI) + * @param moment 回転量(-1 ~ 1) + * @return 1...success 0...failure + */ + bool computeCircular(float r, float rad, float moment); + + /** + * 出力値を取得 + * + * @param wheelNumber wheel番目のホイールの出力を設定 + * + * @return 出力値 + */ + float getOutput(int wheelNumber); +private : +protected : int wheels; - double initDegree; - double wheel[4]; + float wheel[4]; + float radian[4]; }; -#endif /* OMNI */ +#endif//OMNI_H \ No newline at end of file