File content as of revision 9:a30169882a15:

#include "omni.h"

Omni::Omni(int wheels) : wheels(wheels)
{
    for(int i = 0; i < wheels; i++) {
        wheel[i] = 0;
        radian[i] = 0;
    }
}

inline double fmax(double a, double b)
{
    if(a > b) {
        return a;
    } else {
        return b;
    }
}

inline double fmin(double a, double b)
{
    if(a < b) {
        return a;
    } else {
        return b;
    }
}

bool isInRange(float X, float Y)
{
    if(X < -1 || X > 1 || Y < -1 || Y > 1) return 0;
    if(hypot(X, Y) > 1.0) return 0;
    return 1;
}

bool isInRange(float r)
{
    if(r < 0 || r > 1) return 0;
    return 1;
}


bool Omni::setWheelRadian(float rad1, float rad2, float rad3)
{
    if(wheels != 3) return 0;
    radian[0] = rad1;
    radian[1] = rad2;
    radian[2] = rad3;
    return 1;
}

bool Omni::setWheelRadian(float rad1, float rad2, float rad3, float rad4)
{
    if(wheels != 4) return 0;
    radian[0] = rad1;
    radian[1] = rad2;
    radian[2] = rad3;
    radian[3] = rad4;
    return 1;
}

bool Omni::setWheelRadian(int wheelNumber, float rad)
{
    if(wheelNumber > 3|| wheelNumber <= 0) return 0;
    radian[wheelNumber] = rad;
    return 1;
}

bool Omni::computeXY(float X, float Y, float moment)
{
    if(isInRange(X, Y)) {
        return computeCircular(hypot(X, Y), atan2(Y, X), moment);
    } else {
        return 0;
    }
}


bool Omni::computeCircular(float r, float rad, float moment)
{
    if(wheels <= 0) return 0;
    if(isInRange(r)) {
        double parallel[4] = {0};
        double parallelMax = -1;
        double parallelMin = 1;

        for(int i = 0; i < wheels; i++) {
            parallel[i] = sin(rad + radian[i]) * r;
        }
        for(int i = 0; i < wheels; i++) {
            parallelMax = fmax(parallel[i], parallelMax);
            parallelMin = fmin(parallel[i], parallelMin);
        }
        if(parallelMax + moment > 1.0 || parallelMin + moment < -1.0) {
            for(int i = 0; i < wheels; i++) {
                parallel[i] *= (1.0 - fabs(moment));
            }
        }
        for(int i = 0; i < wheels; i++) {
            wheel[i] = parallel[i] + moment;
        }
        return 1;
    } else {
        return 0;
    }
}


float Omni::getOutput(int wheelNumber)
{
    return wheel[wheelNumber];
}