NagaokaRoboticsClub_mbedTeam / omni_wheel

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?

UserRevisionLine numberNew 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 }