sampleProgram
Dependencies: QEI accelerator bit_test cyclic_io cyclic_var cylinder event_var limit mbed mecanum motor_drive pid pid_encoder rs422_put sbdbt servo
Fork of 17robo_fuzi by
Diff: mecanum.h
- Revision:
- 5:6efda58ff71b
- Parent:
- 0:bf96e953cdb8
- Child:
- 10:04f2a82cfd89
--- a/mecanum.h Fri Jun 30 01:18:54 2017 +0000 +++ b/mecanum.h Fri Jun 30 05:40:02 2017 +0000 @@ -1,6 +1,7 @@ /* 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) @@ -11,15 +12,16 @@ .v4 //(返り値[float] -1.0 ~ 1.0) モータ4用出力 メカナム設置図 - ↑→ ←↑ + ↑→ →↓ O---O O---O - ←↑ ↑→ + ←↑ ↓← */ #define rotaPower 0.3 +#define boost_K 0.5 #define PI 3.141592 class Mecanum{ @@ -39,14 +41,22 @@ V3 = vx + vy + rotaPower * (l1 - r1) + w; V4 = -vx - vy + rotaPower * (l1 - r1) + w; - /* - V1 = vx + vy + rotaPower * (r1 - l1) + w; - V2 = vx - vy + rotaPower * (r1 - l1) + w; - V3 = -vx + vy + rotaPower * (r1 - l1) + w; - V4 = -vx - vy + rotaPower * (r1 - l1) + w; - */ } + void boost(){ + if(vy > 0.4){ + V1 += boost_K; + V2 += -boost_K; + V3 += boost_K; + V4 += -boost_K; + }else if(vy < -0.4){ + 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;