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:
- 0:bf96e953cdb8
- Child:
- 5:6efda58ff71b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mecanum.h Mon Jun 26 09:59:14 2017 +0000 @@ -0,0 +1,85 @@ +/* +Mecanum mecanum + +.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.3 +#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; + + /* + 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 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