kusano kiyoshige / Mbed 2 deprecated 17robo_fuzi

ライブラリ化を行った後

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 kusano kiyoshige

mecanum.h@5:6efda58ff71b, 2017-06-30 (annotated)

Committer:
echo_piyo
Date:
Fri Jun 30 05:40:02 2017 +0000
Revision:
5:6efda58ff71b
Parent:
0:bf96e953cdb8
Child:
10:04f2a82cfd89
?????????????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
echo_piyo 0:bf96e953cdb8 1 /*
echo_piyo 0:bf96e953cdb8 2 Mecanum mecanum
echo_piyo 0:bf96e953cdb8 3
echo_piyo 5:6efda58ff71b 4 Sbdbtの入力 +y↑ -y↓ +x→ -x←
echo_piyo 0:bf96e953cdb8 5 .setupdeg([float]Deg) //角度センサの初期値設定
echo_piyo 0:bf96e953cdb8 6 .sbdbt_cal([float]Vx, [float]Vy, [int]con_rl, [int]con_l1, [float]w, [float]angle) //使わない部分は0を代入
echo_piyo 0:bf96e953cdb8 7 .xy_cal([float]Vx, [float]Vy) //xyの代入値(-1.0 ~ 1.0)
echo_piyo 0:bf96e953cdb8 8
echo_piyo 0:bf96e953cdb8 9 .v1 //(返り値[float] -1.0 ~ 1.0) モータ1用出力
echo_piyo 0:bf96e953cdb8 10 .v2 //(返り値[float] -1.0 ~ 1.0) モータ2用出力
echo_piyo 0:bf96e953cdb8 11 .v3 //(返り値[float] -1.0 ~ 1.0) モータ3用出力
echo_piyo 0:bf96e953cdb8 12 .v4 //(返り値[float] -1.0 ~ 1.0) モータ4用出力
echo_piyo 0:bf96e953cdb8 13
echo_piyo 0:bf96e953cdb8 14 メカナム設置図
echo_piyo 5:6efda58ff71b 15 ↑→ →↓
echo_piyo 0:bf96e953cdb8 16 O---O
echo_piyo 0:bf96e953cdb8 17 O---O
echo_piyo 5:6efda58ff71b 18 ←↑ ↓←
echo_piyo 0:bf96e953cdb8 19
echo_piyo 0:bf96e953cdb8 20
echo_piyo 0:bf96e953cdb8 21 */
echo_piyo 0:bf96e953cdb8 22
echo_piyo 0:bf96e953cdb8 23 #define rotaPower 0.3
echo_piyo 5:6efda58ff71b 24 #define boost_K 0.5
echo_piyo 0:bf96e953cdb8 25 #define PI 3.141592
echo_piyo 0:bf96e953cdb8 26
echo_piyo 0:bf96e953cdb8 27 class Mecanum{
echo_piyo 0:bf96e953cdb8 28 public:
echo_piyo 0:bf96e953cdb8 29 void setupdeg(float Deg){
echo_piyo 0:bf96e953cdb8 30 first_deg = Deg;
echo_piyo 0:bf96e953cdb8 31 }
echo_piyo 0:bf96e953cdb8 32
echo_piyo 0:bf96e953cdb8 33 void sbdbt_cal(float Vx, float Vy, int r1, int l1,float w, float angle){
echo_piyo 0:bf96e953cdb8 34 rad = (angle - first_deg)/180*PI;
echo_piyo 0:bf96e953cdb8 35
echo_piyo 0:bf96e953cdb8 36 vx = Vx / 2 * cos(rad) - Vy / 2 * sin(rad);
echo_piyo 0:bf96e953cdb8 37 vy = Vx / 2 * sin(rad) + Vy / 2 * cos(rad);
echo_piyo 0:bf96e953cdb8 38
echo_piyo 0:bf96e953cdb8 39 V1 = -vx + vy + rotaPower * (l1 - r1) + w;
echo_piyo 0:bf96e953cdb8 40 V2 = vx - vy + rotaPower * (l1 - r1) + w;
echo_piyo 0:bf96e953cdb8 41 V3 = vx + vy + rotaPower * (l1 - r1) + w;
echo_piyo 0:bf96e953cdb8 42 V4 = -vx - vy + rotaPower * (l1 - r1) + w;
echo_piyo 0:bf96e953cdb8 43
echo_piyo 0:bf96e953cdb8 44 }
echo_piyo 0:bf96e953cdb8 45
echo_piyo 5:6efda58ff71b 46 void boost(){
echo_piyo 5:6efda58ff71b 47 if(vy > 0.4){
echo_piyo 5:6efda58ff71b 48 V1 += boost_K;
echo_piyo 5:6efda58ff71b 49 V2 += -boost_K;
echo_piyo 5:6efda58ff71b 50 V3 += boost_K;
echo_piyo 5:6efda58ff71b 51 V4 += -boost_K;
echo_piyo 5:6efda58ff71b 52 }else if(vy < -0.4){
echo_piyo 5:6efda58ff71b 53 V1 += -boost_K;
echo_piyo 5:6efda58ff71b 54 V2 += boost_K;
echo_piyo 5:6efda58ff71b 55 V3 += -boost_K;
echo_piyo 5:6efda58ff71b 56 V4 += boost_K;
echo_piyo 5:6efda58ff71b 57 }
echo_piyo 5:6efda58ff71b 58 }
echo_piyo 5:6efda58ff71b 59
echo_piyo 0:bf96e953cdb8 60 void xy_cal(float Vx, float Vy){
echo_piyo 0:bf96e953cdb8 61 V1 = Vx / 2 + Vy / 2;
echo_piyo 0:bf96e953cdb8 62 V2 = Vx / 2 - Vy / 2;
echo_piyo 0:bf96e953cdb8 63 V3 = -Vx / 2 + Vy / 2;
echo_piyo 0:bf96e953cdb8 64 V4 = -Vx / 2 - Vy / 2;
echo_piyo 0:bf96e953cdb8 65 }
echo_piyo 0:bf96e953cdb8 66
echo_piyo 0:bf96e953cdb8 67 float v1(){
echo_piyo 0:bf96e953cdb8 68 return V1;
echo_piyo 0:bf96e953cdb8 69 }
echo_piyo 0:bf96e953cdb8 70
echo_piyo 0:bf96e953cdb8 71 float v2(){
echo_piyo 0:bf96e953cdb8 72 return V2;
echo_piyo 0:bf96e953cdb8 73 }
echo_piyo 0:bf96e953cdb8 74
echo_piyo 0:bf96e953cdb8 75 float v3(){
echo_piyo 0:bf96e953cdb8 76 return V3;
echo_piyo 0:bf96e953cdb8 77 }
echo_piyo 0:bf96e953cdb8 78
echo_piyo 0:bf96e953cdb8 79 float v4(){
echo_piyo 0:bf96e953cdb8 80 return V4;
echo_piyo 0:bf96e953cdb8 81 }
echo_piyo 0:bf96e953cdb8 82
echo_piyo 0:bf96e953cdb8 83 float VX(){
echo_piyo 0:bf96e953cdb8 84 return vx;
echo_piyo 0:bf96e953cdb8 85 }
echo_piyo 0:bf96e953cdb8 86
echo_piyo 0:bf96e953cdb8 87 float VY(){
echo_piyo 0:bf96e953cdb8 88 return vy;
echo_piyo 0:bf96e953cdb8 89 }
echo_piyo 0:bf96e953cdb8 90
echo_piyo 0:bf96e953cdb8 91
echo_piyo 0:bf96e953cdb8 92 private:
echo_piyo 0:bf96e953cdb8 93 float V1, V2, V3, V4, vx, vy;
echo_piyo 0:bf96e953cdb8 94 float first_deg, rad;
echo_piyo 0:bf96e953cdb8 95 };