omni_wheel.cpp

00001 #include "omni_wheel.h"
00002 
00003 OmniWheel::OmniWheel()
00004 {
00005     wheel = new Wheel[4];
00006     wheelNumber = 4;
00007 }
00008 
00009 OmniWheel::OmniWheel(int wheelNumber)
00010     : wheelNumber(wheelNumber)
00011 {
00012     wheel = new Wheel[wheelNumber];
00013 }
00014 
00015 void OmniWheel::computeXY(double X, double Y, double gX, double gY, double moment)
00016 {
00017     computeCircular(hypot(X, Y), atan2(Y, X), gX, gY, moment);
00018 }
00019 
00020 void OmniWheel::computeXY(double X, double Y, double moment)
00021 {
00022     computeCircular(hypot(X, Y), atan2(Y, X), 0, 0, moment);
00023 }
00024 
00025 void OmniWheel::computeCircular(double r, double theta, double gX, double gY, double moment)
00026 {
00027     if(wheelNumber <= 0) return;
00028     double *shiftOut = new double[wheelNumber];
00029     double *rotateOut= new double[wheelNumber];
00030     double shiftMax = -1.0;
00031     double rotateMax = -1.0;
00032     double shiftMin = 1.0;
00033     double rotateMin = 1.0;
00034 
00035     for(int i = 0; i < wheelNumber; i++) {
00036         shiftOut[i] = wheel[i].calculateShift(r, theta);
00037         rotateOut[i] = wheel[i].calculateRotate(gX, gY, moment);
00038         if(shiftOut[i] > shiftMax) shiftMax = shiftOut[i];
00039         if(shiftOut[i] < shiftMin) shiftMin = shiftOut[i];
00040         if(rotateOut[i] > rotateMax) rotateMax = rotateOut[i];
00041         if(rotateOut[i] < rotateMin) rotateMin = rotateOut[i];
00042     }
00043     if (shiftMax + rotateMax > 1.0) {
00044         for (int i = 0; i < wheelNumber; i++) {
00045             wheel[i].setOutput((shiftOut[i] + rotateOut[i]) / fabs(shiftMax + rotateMax));
00046         }
00047     } else if(shiftMin + rotateMin < -1.0) {
00048         for (int i = 0; i < wheelNumber; i++) {
00049             wheel[i].setOutput((shiftOut[i] + rotateOut[i]) / fabs(shiftMin + rotateMin));
00050         }
00051     } else {
00052         for(int i = 0; i < wheelNumber; i++) {
00053             wheel[i].setOutput(shiftOut[i] + rotateOut[i]);
00054         }
00055     }
00056     delete[ ] shiftOut;
00057     delete[ ] rotateOut;
00058 }
00059 
00060 void OmniWheel::computeCircular(double r, double theta, double moment) {
00061     computeCircular(r, theta, 0, 0, moment);
00062 }