omniwheel lib
Dependents: NHK2017_octopus NHK2017_octopus2 NHK2017_octopus2_drive 2018NHK_gakugaku_robo ... more
omni_wheel.cpp@1:e9b590a5b27a, 2017-09-30 (annotated)
- Committer:
- UCHITAKE
- Date:
- Sat Sep 30 05:22:22 2017 +0000
- Revision:
- 1:e9b590a5b27a
- Parent:
- 0:952a0ff1bf46
- Child:
- 3:86fee122b81d
tiny fix & add doc
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
UCHITAKE | 0:952a0ff1bf46 | 1 | #include "omni_wheel.h" |
UCHITAKE | 0:952a0ff1bf46 | 2 | |
UCHITAKE | 0:952a0ff1bf46 | 3 | OmniWheel::OmniWheel() |
UCHITAKE | 0:952a0ff1bf46 | 4 | { |
UCHITAKE | 0:952a0ff1bf46 | 5 | wheel = new Wheel[4]; |
UCHITAKE | 0:952a0ff1bf46 | 6 | wheelNumber = 4; |
UCHITAKE | 0:952a0ff1bf46 | 7 | } |
UCHITAKE | 0:952a0ff1bf46 | 8 | |
UCHITAKE | 0:952a0ff1bf46 | 9 | OmniWheel::OmniWheel(int wheelNumber) |
UCHITAKE | 0:952a0ff1bf46 | 10 | : wheelNumber(wheelNumber) |
UCHITAKE | 0:952a0ff1bf46 | 11 | { |
UCHITAKE | 0:952a0ff1bf46 | 12 | wheel = new Wheel[wheelNumber]; |
UCHITAKE | 0:952a0ff1bf46 | 13 | } |
UCHITAKE | 0:952a0ff1bf46 | 14 | |
UCHITAKE | 0:952a0ff1bf46 | 15 | void OmniWheel::computeXY(double X, double Y, double gX, double gY, double moment) |
UCHITAKE | 0:952a0ff1bf46 | 16 | { |
UCHITAKE | 0:952a0ff1bf46 | 17 | computeCircular(hypot(X, Y), atan2(Y, X), gX, gY, moment); |
UCHITAKE | 0:952a0ff1bf46 | 18 | } |
UCHITAKE | 0:952a0ff1bf46 | 19 | |
UCHITAKE | 0:952a0ff1bf46 | 20 | void OmniWheel::computeXY(double X, double Y, double moment) |
UCHITAKE | 0:952a0ff1bf46 | 21 | { |
UCHITAKE | 0:952a0ff1bf46 | 22 | computeCircular(hypot(X, Y), atan2(Y, X), 0, 0, moment); |
UCHITAKE | 0:952a0ff1bf46 | 23 | } |
UCHITAKE | 0:952a0ff1bf46 | 24 | |
UCHITAKE | 0:952a0ff1bf46 | 25 | void OmniWheel::computeCircular(double r, double theta, double gX, double gY, double moment) |
UCHITAKE | 0:952a0ff1bf46 | 26 | { |
UCHITAKE | 0:952a0ff1bf46 | 27 | if(wheelNumber <= 0) return; |
UCHITAKE | 1:e9b590a5b27a | 28 | double *shiftOut = new double[wheelNumber]; |
UCHITAKE | 1:e9b590a5b27a | 29 | double *rotateOut= new double[wheelNumber]; |
UCHITAKE | 0:952a0ff1bf46 | 30 | double shiftMax = -1.0; |
UCHITAKE | 0:952a0ff1bf46 | 31 | double rotateMax = -1.0; |
UCHITAKE | 0:952a0ff1bf46 | 32 | double shiftMin = 1.0; |
UCHITAKE | 0:952a0ff1bf46 | 33 | double rotateMin = 1.0; |
UCHITAKE | 0:952a0ff1bf46 | 34 | |
UCHITAKE | 0:952a0ff1bf46 | 35 | for(int i = 0; i < wheelNumber; i++) { |
UCHITAKE | 0:952a0ff1bf46 | 36 | shiftOut[i] = wheel[i].calculateShift(r, theta); |
UCHITAKE | 0:952a0ff1bf46 | 37 | rotateOut[i] = wheel[i].calculateRotate(gX, gY, moment); |
UCHITAKE | 0:952a0ff1bf46 | 38 | if(shiftOut[i] > shiftMax) shiftMax = shiftOut[i]; |
UCHITAKE | 0:952a0ff1bf46 | 39 | if(shiftOut[i] < shiftMin) shiftMin = shiftOut[i]; |
UCHITAKE | 0:952a0ff1bf46 | 40 | if(rotateOut[i] > rotateMax) rotateMax = rotateOut[i]; |
UCHITAKE | 0:952a0ff1bf46 | 41 | if(rotateOut[i] < rotateMin) rotateMin = rotateOut[i]; |
UCHITAKE | 0:952a0ff1bf46 | 42 | } |
UCHITAKE | 0:952a0ff1bf46 | 43 | if(shiftMax + rotateMax > 1.0 || shiftMin + shiftMin < -1.0) { |
UCHITAKE | 0:952a0ff1bf46 | 44 | for(int i = 0; i < wheelNumber; i++) { |
UCHITAKE | 0:952a0ff1bf46 | 45 | wheel[i].setOutput( |
UCHITAKE | 0:952a0ff1bf46 | 46 | (shiftOut[i] + rotateOut[i]) * |
UCHITAKE | 0:952a0ff1bf46 | 47 | 1.0 / ( |
UCHITAKE | 0:952a0ff1bf46 | 48 | ((fabs(shiftMax) > fabs(shiftMin))?fabs(shiftMax):fabs(shiftMin)) + |
UCHITAKE | 0:952a0ff1bf46 | 49 | ((fabs(rotateMax) > fabs(rotateMin))?fabs(rotateMax):fabs(rotateMin)) |
UCHITAKE | 0:952a0ff1bf46 | 50 | ) |
UCHITAKE | 0:952a0ff1bf46 | 51 | ); |
UCHITAKE | 0:952a0ff1bf46 | 52 | } |
UCHITAKE | 0:952a0ff1bf46 | 53 | } else { |
UCHITAKE | 0:952a0ff1bf46 | 54 | for(int i = 0; i < wheelNumber; i++) { |
UCHITAKE | 0:952a0ff1bf46 | 55 | wheel[i].setOutput(shiftOut[i] + rotateOut[i]); |
UCHITAKE | 0:952a0ff1bf46 | 56 | } |
UCHITAKE | 0:952a0ff1bf46 | 57 | } |
UCHITAKE | 1:e9b590a5b27a | 58 | delete[ ] shiftOut; |
UCHITAKE | 1:e9b590a5b27a | 59 | delete[ ] rotateOut; |
UCHITAKE | 1:e9b590a5b27a | 60 | } |