File content as of revision 1:e515f6a4da2e:

#include "mbed.h"
#include "math.h"
#include "OmniMove.h"

void OmniMove::setup(int nWheel,float fstWheelAng){
    this->nWheel = nWheel;
    m_pi = 3.1415;
    
    for(i = 0;i < nWheel; i++){
        Vx_wheel[i] = -1 * sin( conv_rad( fstWheelAng + (360 / nWheel) * i ) );
        Vy_wheel[i] = cos( conv_rad( fstWheelAng + (360 / nWheel) * i ) );
    }
}

void OmniMove::input_polar(float r,float theta,float Vroll,float roll_ratio){
    Vx = r * cos( conv_rad( theta ) ) * ( 1 - roll_ratio );
    Vy = r * sin( conv_rad( theta ) ) * ( 1 - roll_ratio );
    this->Vroll = Vroll * roll_ratio;
}

void OmniMove::input_polar(float r,float theta,float Vroll,float roll_ratio,float MachineAng){
    Vx = r * cos( conv_rad( theta - MachineAng ) ) * ( 1 - roll_ratio );
    Vy = r * sin( conv_rad( theta - MachineAng ) ) * ( 1 - roll_ratio );
    this->Vroll = Vroll * roll_ratio;
}

float OmniMove::output(int nWheel){
    if( nWheel < 8 ){
        return Vx*Vx_wheel[nWheel] + Vy*Vy_wheel[nWheel] + Vroll;
    }else{
        return 0;
    }
}

void OmniMove::output(float *V){
    for(i = 0;i < nWheel;i++){
        V[i] = Vx*Vx_wheel[i] + Vy*Vy_wheel[i] + Vroll;
    }
}

float OmniMove::conv_deg(float _rad){
    return _rad * 180 / m_pi;
}

float OmniMove::conv_rad(float _deg){
    return m_pi * _deg / 180;
}