NagaokaRoboticsClub_mbedTeam / omni_wheel

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?

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