omni wheel library
Dependents: quadOmni_yanagi NHK2017_octopus hayatoShooter
オムニ用のライブラリです。
omni.cpp@10:4f0b55344c73, 2017-08-23 (annotated)
- Committer:
- UCHITAKE
- Date:
- Wed Aug 23 07:26:26 2017 +0000
- Revision:
- 10:4f0b55344c73
- Parent:
- 8:ab46e745cfab
fix commit
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
UCHITAKE | 0:979565e955a5 | 1 | #include "omni.h" |
UCHITAKE | 0:979565e955a5 | 2 | |
UCHITAKE | 10:4f0b55344c73 | 3 | Omni::Omni(int wheels) : wheels(wheels) |
UCHITAKE | 10:4f0b55344c73 | 4 | { |
UCHITAKE | 10:4f0b55344c73 | 5 | for(int i = 0; i < wheels; i++) { |
UCHITAKE | 10:4f0b55344c73 | 6 | wheel[i] = 0; |
UCHITAKE | 10:4f0b55344c73 | 7 | radian[i] = 0; |
UCHITAKE | 10:4f0b55344c73 | 8 | } |
UCHITAKE | 10:4f0b55344c73 | 9 | } |
UCHITAKE | 10:4f0b55344c73 | 10 | |
UCHITAKE | 0:979565e955a5 | 11 | inline double fmax(double a, double b) |
UCHITAKE | 0:979565e955a5 | 12 | { |
UCHITAKE | 0:979565e955a5 | 13 | if(a > b) { |
UCHITAKE | 0:979565e955a5 | 14 | return a; |
UCHITAKE | 0:979565e955a5 | 15 | } else { |
UCHITAKE | 0:979565e955a5 | 16 | return b; |
UCHITAKE | 0:979565e955a5 | 17 | } |
UCHITAKE | 0:979565e955a5 | 18 | } |
UCHITAKE | 0:979565e955a5 | 19 | |
UCHITAKE | 0:979565e955a5 | 20 | inline double fmin(double a, double b) |
UCHITAKE | 0:979565e955a5 | 21 | { |
UCHITAKE | 0:979565e955a5 | 22 | if(a < b) { |
UCHITAKE | 0:979565e955a5 | 23 | return a; |
UCHITAKE | 0:979565e955a5 | 24 | } else { |
UCHITAKE | 0:979565e955a5 | 25 | return b; |
UCHITAKE | 0:979565e955a5 | 26 | } |
UCHITAKE | 0:979565e955a5 | 27 | } |
UCHITAKE | 0:979565e955a5 | 28 | |
UCHITAKE | 10:4f0b55344c73 | 29 | bool isInRange(float X, float Y) |
UCHITAKE | 10:4f0b55344c73 | 30 | { |
UCHITAKE | 10:4f0b55344c73 | 31 | if(X < -1 || X > 1 || Y < -1 || Y > 1) return 0; |
UCHITAKE | 10:4f0b55344c73 | 32 | if(hypot(X, Y) > 1.0) return 0; |
UCHITAKE | 10:4f0b55344c73 | 33 | return 1; |
UCHITAKE | 10:4f0b55344c73 | 34 | } |
UCHITAKE | 10:4f0b55344c73 | 35 | |
UCHITAKE | 10:4f0b55344c73 | 36 | bool isInRange(float r) |
UCHITAKE | 10:4f0b55344c73 | 37 | { |
UCHITAKE | 10:4f0b55344c73 | 38 | if(r < 0 || r > 1) return 0; |
UCHITAKE | 10:4f0b55344c73 | 39 | return 1; |
UCHITAKE | 10:4f0b55344c73 | 40 | } |
UCHITAKE | 10:4f0b55344c73 | 41 | |
UCHITAKE | 10:4f0b55344c73 | 42 | |
UCHITAKE | 10:4f0b55344c73 | 43 | bool Omni::setWheelRadian(float rad1, float rad2, float rad3) |
UCHITAKE | 10:4f0b55344c73 | 44 | { |
UCHITAKE | 10:4f0b55344c73 | 45 | if(wheels != 3) return 0; |
UCHITAKE | 10:4f0b55344c73 | 46 | radian[0] = rad1; |
UCHITAKE | 10:4f0b55344c73 | 47 | radian[1] = rad2; |
UCHITAKE | 10:4f0b55344c73 | 48 | radian[2] = rad3; |
UCHITAKE | 10:4f0b55344c73 | 49 | return 1; |
UCHITAKE | 0:979565e955a5 | 50 | } |
UCHITAKE | 0:979565e955a5 | 51 | |
UCHITAKE | 10:4f0b55344c73 | 52 | bool Omni::setWheelRadian(float rad1, float rad2, float rad3, float rad4) |
UCHITAKE | 10:4f0b55344c73 | 53 | { |
UCHITAKE | 10:4f0b55344c73 | 54 | if(wheels != 4) return 0; |
UCHITAKE | 10:4f0b55344c73 | 55 | radian[0] = rad1; |
UCHITAKE | 10:4f0b55344c73 | 56 | radian[1] = rad2; |
UCHITAKE | 10:4f0b55344c73 | 57 | radian[2] = rad3; |
UCHITAKE | 10:4f0b55344c73 | 58 | radian[3] = rad4; |
UCHITAKE | 10:4f0b55344c73 | 59 | return 1; |
UCHITAKE | 7:1d0aa4275eb0 | 60 | } |
UCHITAKE | 7:1d0aa4275eb0 | 61 | |
UCHITAKE | 10:4f0b55344c73 | 62 | bool Omni::setWheelRadian(int wheelNumber, float rad) |
UCHITAKE | 0:979565e955a5 | 63 | { |
UCHITAKE | 10:4f0b55344c73 | 64 | if(wheelNumber > 3|| wheelNumber <= 0) return 0; |
UCHITAKE | 10:4f0b55344c73 | 65 | radian[wheelNumber] = rad; |
UCHITAKE | 10:4f0b55344c73 | 66 | return 1; |
UCHITAKE | 10:4f0b55344c73 | 67 | } |
UCHITAKE | 10:4f0b55344c73 | 68 | |
UCHITAKE | 10:4f0b55344c73 | 69 | bool Omni::computeXY(float X, float Y, float moment) |
UCHITAKE | 10:4f0b55344c73 | 70 | { |
UCHITAKE | 10:4f0b55344c73 | 71 | if(isInRange(X, Y)) { |
UCHITAKE | 10:4f0b55344c73 | 72 | return computeCircular(hypot(X, Y), atan2(Y, X), moment); |
UCHITAKE | 10:4f0b55344c73 | 73 | } else { |
UCHITAKE | 10:4f0b55344c73 | 74 | return 0; |
UCHITAKE | 0:979565e955a5 | 75 | } |
UCHITAKE | 0:979565e955a5 | 76 | } |
UCHITAKE | 0:979565e955a5 | 77 | |
UCHITAKE | 10:4f0b55344c73 | 78 | |
UCHITAKE | 10:4f0b55344c73 | 79 | bool Omni::computeCircular(float r, float rad, float moment) |
UCHITAKE | 0:979565e955a5 | 80 | { |
UCHITAKE | 10:4f0b55344c73 | 81 | if(wheels <= 0) return 0; |
UCHITAKE | 10:4f0b55344c73 | 82 | if(isInRange(r)) { |
UCHITAKE | 10:4f0b55344c73 | 83 | double parallel[4] = {0}; |
UCHITAKE | 10:4f0b55344c73 | 84 | double parallelMax = -1; |
UCHITAKE | 10:4f0b55344c73 | 85 | double parallelMin = 1; |
UCHITAKE | 0:979565e955a5 | 86 | |
UCHITAKE | 10:4f0b55344c73 | 87 | for(int i = 0; i < wheels; i++) { |
UCHITAKE | 10:4f0b55344c73 | 88 | parallel[i] = sin(rad + radian[i]) * r; |
UCHITAKE | 10:4f0b55344c73 | 89 | } |
UCHITAKE | 10:4f0b55344c73 | 90 | for(int i = 0; i < wheels; i++) { |
UCHITAKE | 10:4f0b55344c73 | 91 | parallelMax = fmax(parallel[i], parallelMax); |
UCHITAKE | 10:4f0b55344c73 | 92 | parallelMin = fmin(parallel[i], parallelMin); |
UCHITAKE | 10:4f0b55344c73 | 93 | } |
UCHITAKE | 10:4f0b55344c73 | 94 | if(parallelMax + moment > 1.0 || parallelMin + moment < -1.0) { |
UCHITAKE | 10:4f0b55344c73 | 95 | for(int i = 0; i < wheels; i++) { |
UCHITAKE | 10:4f0b55344c73 | 96 | parallel[i] *= (1.0 - fabs(moment)); |
UCHITAKE | 10:4f0b55344c73 | 97 | } |
UCHITAKE | 10:4f0b55344c73 | 98 | } |
UCHITAKE | 10:4f0b55344c73 | 99 | for(int i = 0; i < wheels; i++) { |
UCHITAKE | 10:4f0b55344c73 | 100 | wheel[i] = parallel[i] + moment; |
UCHITAKE | 10:4f0b55344c73 | 101 | } |
UCHITAKE | 10:4f0b55344c73 | 102 | return 1; |
UCHITAKE | 10:4f0b55344c73 | 103 | } else { |
UCHITAKE | 0:979565e955a5 | 104 | return 0; |
UCHITAKE | 0:979565e955a5 | 105 | } |
UCHITAKE | 0:979565e955a5 | 106 | } |
UCHITAKE | 0:979565e955a5 | 107 | |
UCHITAKE | 0:979565e955a5 | 108 | |
UCHITAKE | 10:4f0b55344c73 | 109 | float Omni::getOutput(int wheelNumber) |
UCHITAKE | 0:979565e955a5 | 110 | { |
UCHITAKE | 10:4f0b55344c73 | 111 | return wheel[wheelNumber]; |
UCHITAKE | 10:4f0b55344c73 | 112 | } |