omniwheel lib
Dependents: NHK2017_octopus NHK2017_octopus2 NHK2017_octopus2_drive 2018NHK_gakugaku_robo ... more
omni_wheel.cpp@5:cfec945ea421, 2019-01-21 (annotated)
- Committer:
- shinjitakano
- Date:
- Mon Jan 21 17:57:44 2019 +0900
- Revision:
- 5:cfec945ea421
- Parent:
- 4:6005d2ab5d52
wheel.cpp???????
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 | } |
takeuchi |
3:86fee122b81d | 43 | if (shiftMax + rotateMax > 1.0) { |
takeuchi |
3:86fee122b81d | 44 | for (int i = 0; i < wheelNumber; i++) { |
takeuchi |
4:6005d2ab5d52 | 45 | wheel[i].setOutput((shiftOut[i] + rotateOut[i]) / fabs(shiftMax + rotateMax)); |
takeuchi |
3:86fee122b81d | 46 | } |
takeuchi |
3:86fee122b81d | 47 | } else if(shiftMin + rotateMin < -1.0) { |
takeuchi |
3:86fee122b81d | 48 | for (int i = 0; i < wheelNumber; i++) { |
takeuchi |
4:6005d2ab5d52 | 49 | wheel[i].setOutput((shiftOut[i] + rotateOut[i]) / fabs(shiftMin + rotateMin)); |
UCHITAKE | 0:952a0ff1bf46 | 50 | } |
UCHITAKE | 0:952a0ff1bf46 | 51 | } else { |
UCHITAKE | 0:952a0ff1bf46 | 52 | for(int i = 0; i < wheelNumber; i++) { |
UCHITAKE | 0:952a0ff1bf46 | 53 | wheel[i].setOutput(shiftOut[i] + rotateOut[i]); |
UCHITAKE | 0:952a0ff1bf46 | 54 | } |
UCHITAKE | 0:952a0ff1bf46 | 55 | } |
UCHITAKE | 1:e9b590a5b27a | 56 | delete[ ] shiftOut; |
UCHITAKE | 1:e9b590a5b27a | 57 | delete[ ] rotateOut; |
takeuchi |
3:86fee122b81d | 58 | } |
takeuchi |
3:86fee122b81d | 59 | |
takeuchi |
3:86fee122b81d | 60 | void OmniWheel::computeCircular(double r, double theta, double moment) { |
takeuchi |
3:86fee122b81d | 61 | computeCircular(r, theta, 0, 0, moment); |
takeuchi |
3:86fee122b81d | 62 | } |