mecanum_.h

00001 /*
00002 Mecanum mecanum
00003 
00004 Sbdbtの入力 +y↑ -y↓ +x→ -x←
00005 .setupdeg([float]Deg)   //角度センサの初期値設定
00006 .sbdbt_cal([float]Vx, [float]Vy, [int]con_rl, [int]con_l1, [float]w, [float]angle) //使わない部分は0を代入
00007 .xy_cal([float]Vx, [float]Vy) //xyの代入値(-1.0 ~ 1.0) 
00008 
00009 .v1 //(返り値[float] -1.0 ~ 1.0) モータ1用出力
00010 .v2 //(返り値[float] -1.0 ~ 1.0) モータ2用出力
00011 .v3 //(返り値[float] -1.0 ~ 1.0) モータ3用出力
00012 .v4 //(返り値[float] -1.0 ~ 1.0) モータ4用出力
00013 
00014 メカナム設置図
00015     ↑→   →↓
00016      O---O
00017      O---O
00018     ←↑   ↓←
00019 
00020 
00021 */
00022 
00023 #define rotaPower 0.6
00024 #define rotaStickPower 0.35
00025 #define boost_A 0.5
00026 #define boost_K 0.9
00027 #define PI 3.141592
00028 
00029 class Mecanum{
00030     public:
00031         void setupdeg(float Deg){
00032                 first_deg = Deg;
00033         }
00034     
00035         void sbdbt_cal(float Vx, float Vy, int r1, int l1,float w, float angle){
00036             rad = (angle - first_deg)/180*PI;
00037             
00038             vx = Vx / 2 * cos(rad) - Vy / 2 * sin(rad);
00039             vy = Vx / 2 * sin(rad) + Vy / 2 * cos(rad);
00040 
00041             V1 =   vx + vy + rotaPower * (l1 - r1) + rotaStickPower * w;
00042             V2 =  -vx - vy + rotaPower * (l1 - r1) + rotaStickPower * w;
00043             V3 =  -vx + vy + rotaPower * (l1 - r1) + rotaStickPower * w;
00044             V4 =   vx - vy + rotaPower * (l1 - r1) + rotaStickPower * w;
00045             
00046         }
00047         
00048         void boost(){
00049             if(vy > 0.4){
00050                 V1 +=  boost_A;
00051                 V2 += -boost_A;
00052                 V3 +=  boost_A;
00053                 V4 += -boost_A;
00054             }else if(vy < -0.4){
00055                 V1 += -boost_A;
00056                 V2 +=  boost_A;
00057                 V3 += -boost_A;
00058                 V4 +=  boost_A;
00059             }
00060         }
00061         
00062         void boost_forward(){
00063                 V1 = -boost_K;
00064                 V2 =  boost_K;
00065                 V3 = -boost_K;
00066                 V4 =  boost_K;
00067         }
00068         
00069         void boost_back(){
00070                 V1 =  boost_K;
00071                 V2 = -boost_K;
00072                 V3 =  boost_K;
00073                 V4 = -boost_K;
00074         }
00075         
00076         void boost_right(){
00077                 V1 =  boost_K;
00078                 V2 = -boost_K;
00079                 V3 = -boost_K;
00080                 V4 =  boost_K;
00081         }
00082         
00083         void boost_left(){
00084                 V1 = -boost_K;
00085                 V2 =  boost_K;
00086                 V3 =  boost_K;
00087                 V4 = -boost_K;
00088         }
00089         
00090         
00091         void xy_cal(float Vx, float Vy){
00092             V1 =  Vx / 2 + Vy / 2;
00093             V2 =  Vx / 2 - Vy / 2;
00094             V3 = -Vx / 2 + Vy / 2;
00095             V4 = -Vx / 2 - Vy / 2;
00096         }
00097         
00098         float v1(){
00099             return V1; 
00100         } 
00101         
00102         float v2(){
00103             return V2;  
00104         } 
00105         
00106         float v3(){
00107             return V3;  
00108         } 
00109         
00110         float v4(){
00111             return V4;
00112         }
00113         
00114         float VX(){
00115             return vx;
00116         } 
00117         
00118         float VY(){
00119             return vy;
00120         } 
00121         
00122         
00123     private:
00124         float V1, V2, V3, V4, vx, vy;
00125         float first_deg, rad;
00126 };