![](/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
headerfile_unuse/mecanum_.h
- Committer:
- echo_piyo
- Date:
- 2017-09-24
- Revision:
- 66:1664ee92539d
- Parent:
- mecanum.h@ 56:a7bd860b85b6
File content as of revision 66:1664ee92539d:
/* 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; };