File content as of revision 0:f3c5ba43a1aa:

#include "mecanum.h"
#include "math.h"

void Mecanum::setupdeg(float Deg){
    first_deg = Deg;
}
void Mecanum::sbdbt_cal(float Vx, float Vy, int r1, int l1,float w, float angle){
    rad = (angle - first_deg)/180*PI;

    vx = Vx / 2 * cos(rad) - Vy / 2 * sin(rad);
    vy = Vx / 2 * sin(rad) + Vy / 2 * cos(rad);

    V1 =   vx + vy + rotaPower * (l1 - r1) + rotaStickPower * w;
    V2 =  -vx - vy + rotaPower * (l1 - r1) + rotaStickPower * w;
    V3 =  -vx + vy + rotaPower * (l1 - r1) + rotaStickPower * w;
    V4 =   vx - vy + rotaPower * (l1 - r1) + rotaStickPower * w;
}
void Mecanum::boost(){
    if(vy > 0.4) {
        V1 +=  boost_A;
        V2 += -boost_A;
        V3 +=  boost_A;
        V4 += -boost_A;
    } else if(vy < -0.4) {
        V1 += -boost_A;
        V2 +=  boost_A;
        V3 += -boost_A;
        V4 +=  boost_A;
    }
}
void Mecanum::boost_forward(){
    V1 = -boost_K;
    V2 =  boost_K;
    V3 = -boost_K;
    V4 =  boost_K;
}
void Mecanum::boost_back(){
    V1 =  boost_K;
    V2 = -boost_K;
    V3 =  boost_K;
    V4 = -boost_K;
}
void Mecanum::boost_right(){
    V1 =  boost_K;
    V2 = -boost_K;
    V3 = -boost_K;
    V4 =  boost_K;
}
void Mecanum::boost_left(){
    V1 = -boost_K;
    V2 =  boost_K;
    V3 =  boost_K;
    V4 = -boost_K;
}
void Mecanum::xy_cal(float Vx, float Vy){
    V1 =  Vx / 2 + Vy / 2;
    V2 =  Vx / 2 - Vy / 2;
    V3 = -Vx / 2 + Vy / 2;
    V4 = -Vx / 2 - Vy / 2;
}
float Mecanum::v1(){
    return V1;
}
float Mecanum::v2(){
    return V2;
}
float Mecanum::v3(){
    return V3;
}
float Mecanum::v4(){
    return V4;
}
float Mecanum::VX(){
    return vx;
}
float Mecanum::VY(){
    return vy;
}