File content as of revision 10:04f2a82cfd89:

/*
Mecanum mecanum

Sbdbtの入力 +y↑ -y↓ +x→ -x←
.setupdeg([float]Deg)   //角度センサの初期値設定
.sbdbt_cal([float]Vx, [float]Vy, [int]con_rl, [int]con_l1, [float]w, [float]angle) //使わない部分は0を代入
.xy_cal([float]Vx, [float]Vy) //xyの代入値(-1.0 ~ 1.0) 

.v1 //(返り値[float] -1.0 ~ 1.0) モータ1用出力
.v2 //(返り値[float] -1.0 ~ 1.0) モータ2用出力
.v3 //(返り値[float] -1.0 ~ 1.0) モータ3用出力
.v4 //(返り値[float] -1.0 ~ 1.0) モータ4用出力

メカナム設置図
    ↑→   →↓
     O---O
     O---O
    ←↑   ↓←


*/

#define rotaPower 0.4
#define boost_A 0.5
#define boost_K 0.9
#define PI 3.141592

class Mecanum{
    public:
        void setupdeg(float Deg){
                first_deg = Deg;
        }
    
        void 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) + w;
            V2 =   vx - vy + rotaPower * (l1 - r1) + w;
            V3 =   vx + vy + rotaPower * (l1 - r1) + w;
            V4 =  -vx - vy + rotaPower * (l1 - r1) + w;
            
        }
        
        void 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 boost_forward(){
                V1 = -boost_K;
                V2 =  boost_K;
                V3 = -boost_K;
                V4 =  boost_K;
        }
        
        void boost_back(){
                V1 =  boost_K;
                V2 = -boost_K;
                V3 =  boost_K;
                V4 = -boost_K;
        }
        
        void boost_right(){
                V1 =  boost_K;
                V2 = -boost_K;
                V3 = -boost_K;
                V4 =  boost_K;
        }
        
        void boost_left(){
                V1 = -boost_K;
                V2 =  boost_K;
                V3 =  boost_K;
                V4 = -boost_K;
        }
        
        
        void 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 v1(){
            return V1; 
        } 
        
        float v2(){
            return V2;  
        } 
        
        float v3(){
            return V3;  
        } 
        
        float v4(){
            return V4;
        }
        
        float VX(){
            return vx;
        } 
        
        float VY(){
            return vy;
        } 
        
        
    private:
        float V1, V2, V3, V4, vx, vy;
        float first_deg, rad;
};