omni wheel library
Dependents: quadOmni_yanagi NHK2017_octopus hayatoShooter
オムニ用のライブラリです。
omni.cpp@8:ab46e745cfab, 2017-08-06 (annotated)
- Committer:
- UCHITAKE
- Date:
- Sun Aug 06 14:49:01 2017 +0000
- Revision:
- 8:ab46e745cfab
- Parent:
- 7:1d0aa4275eb0
- Child:
- 10:4f0b55344c73
fix bug
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 | 0:979565e955a5 | 3 | inline double fmax(double a, double b) |
UCHITAKE | 0:979565e955a5 | 4 | { |
UCHITAKE | 0:979565e955a5 | 5 | if(a > b) { |
UCHITAKE | 0:979565e955a5 | 6 | return a; |
UCHITAKE | 0:979565e955a5 | 7 | } else { |
UCHITAKE | 0:979565e955a5 | 8 | return b; |
UCHITAKE | 0:979565e955a5 | 9 | } |
UCHITAKE | 0:979565e955a5 | 10 | } |
UCHITAKE | 0:979565e955a5 | 11 | |
UCHITAKE | 0:979565e955a5 | 12 | inline double fmin(double a, double b) |
UCHITAKE | 0:979565e955a5 | 13 | { |
UCHITAKE | 0:979565e955a5 | 14 | if(a < b) { |
UCHITAKE | 0:979565e955a5 | 15 | return a; |
UCHITAKE | 0:979565e955a5 | 16 | } else { |
UCHITAKE | 0:979565e955a5 | 17 | return b; |
UCHITAKE | 0:979565e955a5 | 18 | } |
UCHITAKE | 0:979565e955a5 | 19 | } |
UCHITAKE | 0:979565e955a5 | 20 | |
UCHITAKE | 0:979565e955a5 | 21 | inline double toRadians(double degrees) { |
UCHITAKE | 0:979565e955a5 | 22 | return degrees * (M_PI / 180.0); |
UCHITAKE | 0:979565e955a5 | 23 | } |
UCHITAKE | 0:979565e955a5 | 24 | |
UCHITAKE | 7:1d0aa4275eb0 | 25 | inline double toDegrees(double radians) { |
UCHITAKE | 8:ab46e745cfab | 26 | return radians * (180.0 / M_PI); |
UCHITAKE | 7:1d0aa4275eb0 | 27 | } |
UCHITAKE | 7:1d0aa4275eb0 | 28 | |
UCHITAKE | 0:979565e955a5 | 29 | Omni::Omni(int wheels, double initDegree) : |
UCHITAKE | 0:979565e955a5 | 30 | wheels(wheels), initDegree(initDegree) |
UCHITAKE | 0:979565e955a5 | 31 | { |
UCHITAKE | 0:979565e955a5 | 32 | for(int i = 0; i < wheels; i++) { |
UCHITAKE | 0:979565e955a5 | 33 | wheel[i] = 0; |
UCHITAKE | 0:979565e955a5 | 34 | } |
UCHITAKE | 0:979565e955a5 | 35 | } |
UCHITAKE | 0:979565e955a5 | 36 | |
UCHITAKE | 0:979565e955a5 | 37 | bool Omni::computeXY(double parallelVector[], double moment) |
UCHITAKE | 0:979565e955a5 | 38 | { |
UCHITAKE | 0:979565e955a5 | 39 | if(wheels < 3 || wheels > 4) { |
UCHITAKE | 0:979565e955a5 | 40 | return 0; |
UCHITAKE | 0:979565e955a5 | 41 | } |
UCHITAKE | 0:979565e955a5 | 42 | |
UCHITAKE | 0:979565e955a5 | 43 | double polar[2]; |
UCHITAKE | 0:979565e955a5 | 44 | |
UCHITAKE | 7:1d0aa4275eb0 | 45 | polar[0] = toDegrees(atan2(parallelVector[1], parallelVector[0])); |
UCHITAKE | 0:979565e955a5 | 46 | polar[1] = hypot(parallelVector[0], parallelVector[1]); |
UCHITAKE | 0:979565e955a5 | 47 | |
UCHITAKE | 0:979565e955a5 | 48 | return computePolar(polar, moment); |
UCHITAKE | 0:979565e955a5 | 49 | } |
UCHITAKE | 0:979565e955a5 | 50 | |
UCHITAKE | 0:979565e955a5 | 51 | bool Omni::computePolar(double parallelVector[], double moment) |
UCHITAKE | 0:979565e955a5 | 52 | { |
UCHITAKE | 0:979565e955a5 | 53 | if(wheels < 3 || wheels > 4) { |
UCHITAKE | 0:979565e955a5 | 54 | return 0; |
UCHITAKE | 0:979565e955a5 | 55 | } |
UCHITAKE | 0:979565e955a5 | 56 | |
UCHITAKE | 0:979565e955a5 | 57 | if(parallelVector[1] < 0 || parallelVector[1] > 1.0) { |
UCHITAKE | 0:979565e955a5 | 58 | return 0; |
UCHITAKE | 0:979565e955a5 | 59 | } |
UCHITAKE | 0:979565e955a5 | 60 | |
UCHITAKE | 0:979565e955a5 | 61 | double parallel[4] = {0}; |
UCHITAKE | 6:59171f3235b6 | 62 | double parallelMax = -1; |
UCHITAKE | 6:59171f3235b6 | 63 | double parallelMin = 1; |
UCHITAKE | 0:979565e955a5 | 64 | |
UCHITAKE | 0:979565e955a5 | 65 | for(int i = 0; i < wheels; i++) { |
UCHITAKE | 5:f2fc4a0b6a6f | 66 | parallel[i] = sin(toRadians(parallelVector[0] + initDegree + (360 / wheels) * i)) * parallelVector[1]; |
UCHITAKE | 0:979565e955a5 | 67 | } |
UCHITAKE | 6:59171f3235b6 | 68 | for(int i = 0; i < wheels; i++) { |
UCHITAKE | 6:59171f3235b6 | 69 | parallelMax = fmax(parallel[i], parallelMax); |
UCHITAKE | 6:59171f3235b6 | 70 | parallelMin = fmin(parallel[i], parallelMin); |
UCHITAKE | 0:979565e955a5 | 71 | } |
UCHITAKE | 0:979565e955a5 | 72 | if(parallelMax + moment > 1.0 || parallelMin + moment < -1.0) { |
UCHITAKE | 0:979565e955a5 | 73 | for(int i = 0; i < wheels; i++) { |
UCHITAKE | 0:979565e955a5 | 74 | parallel[i] *= (1.0 - fabs(moment)); |
UCHITAKE | 0:979565e955a5 | 75 | } |
UCHITAKE | 0:979565e955a5 | 76 | } |
UCHITAKE | 0:979565e955a5 | 77 | for(int i = 0; i < wheels; i++) { |
UCHITAKE | 0:979565e955a5 | 78 | wheel[i] = parallel[i] + moment; |
UCHITAKE | 0:979565e955a5 | 79 | } |
UCHITAKE | 0:979565e955a5 | 80 | return 1; |
UCHITAKE | 0:979565e955a5 | 81 | } |
UCHITAKE | 0:979565e955a5 | 82 | |
UCHITAKE | 0:979565e955a5 | 83 | bool Omni::stop() |
UCHITAKE | 0:979565e955a5 | 84 | { |
UCHITAKE | 0:979565e955a5 | 85 | for(int i = 0; i < wheels; i++) { |
UCHITAKE | 0:979565e955a5 | 86 | wheel[i] = 0; |
UCHITAKE | 0:979565e955a5 | 87 | } |
UCHITAKE | 0:979565e955a5 | 88 | return 1; |
UCHITAKE | 0:979565e955a5 | 89 | } |
UCHITAKE | 0:979565e955a5 | 90 | |
UCHITAKE | 0:979565e955a5 | 91 | void Omni::setWheels(int wheel) |
UCHITAKE | 0:979565e955a5 | 92 | { |
UCHITAKE | 0:979565e955a5 | 93 | wheels = wheel; |
UCHITAKE | 0:979565e955a5 | 94 | } |
UCHITAKE | 0:979565e955a5 | 95 | |
UCHITAKE | 0:979565e955a5 | 96 | void Omni::setInitDegree(double degree) |
UCHITAKE | 0:979565e955a5 | 97 | { |
UCHITAKE | 0:979565e955a5 | 98 | initDegree = degree; |
UCHITAKE | 0:979565e955a5 | 99 | } |
UCHITAKE | 0:979565e955a5 | 100 | |
UCHITAKE | 0:979565e955a5 | 101 | double Omni::getOutput(int num) |
UCHITAKE | 0:979565e955a5 | 102 | { |
UCHITAKE | 0:979565e955a5 | 103 | return wheel[num]; |
UCHITAKE | 0:979565e955a5 | 104 | } |