![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ライブラリ化を行った後
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_Practice1 by
Diff: headerfile_unuse/mecanum_.h
- Revision:
- 66:1664ee92539d
- Parent:
- 56:a7bd860b85b6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/headerfile_unuse/mecanum_.h Sun Sep 24 05:25:03 2017 +0000 @@ -0,0 +1,126 @@ +/* +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.6 +#define rotaStickPower 0.35 +#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) + 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 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; +}; \ No newline at end of file