![](/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
mecanum.h@55:2dd2f161ebaf, 2017-09-16 (annotated)
- Committer:
- echo_piyo
- Date:
- Sat Sep 16 05:59:00 2017 +0000
- Revision:
- 55:2dd2f161ebaf
- Parent:
- 52:f5ae47e683fa
- Child:
- 56:a7bd860b85b6
(?)???????(700); ??????x?????????
Who changed what in which revision?
User | Revision | Line number | New 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 | 52:f5ae47e683fa | 23 | #define rotaPower 0.6 |
echo_piyo | 55:2dd2f161ebaf | 24 | #define rotaStickPower 0.2 |
echo_piyo | 10:04f2a82cfd89 | 25 | #define boost_A 0.5 |
echo_piyo | 10:04f2a82cfd89 | 26 | #define boost_K 0.9 |
echo_piyo | 0:bf96e953cdb8 | 27 | #define PI 3.141592 |
echo_piyo | 0:bf96e953cdb8 | 28 | |
echo_piyo | 0:bf96e953cdb8 | 29 | class Mecanum{ |
echo_piyo | 0:bf96e953cdb8 | 30 | public: |
echo_piyo | 0:bf96e953cdb8 | 31 | void setupdeg(float Deg){ |
echo_piyo | 0:bf96e953cdb8 | 32 | first_deg = Deg; |
echo_piyo | 0:bf96e953cdb8 | 33 | } |
echo_piyo | 0:bf96e953cdb8 | 34 | |
echo_piyo | 0:bf96e953cdb8 | 35 | void sbdbt_cal(float Vx, float Vy, int r1, int l1,float w, float angle){ |
echo_piyo | 0:bf96e953cdb8 | 36 | rad = (angle - first_deg)/180*PI; |
echo_piyo | 0:bf96e953cdb8 | 37 | |
echo_piyo | 0:bf96e953cdb8 | 38 | vx = Vx / 2 * cos(rad) - Vy / 2 * sin(rad); |
echo_piyo | 0:bf96e953cdb8 | 39 | vy = Vx / 2 * sin(rad) + Vy / 2 * cos(rad); |
echo_piyo | 0:bf96e953cdb8 | 40 | |
echo_piyo | 55:2dd2f161ebaf | 41 | V1 = vx + vy + rotaPower * (l1 - r1) + rotaStickPower * w; |
echo_piyo | 55:2dd2f161ebaf | 42 | V2 = -vx - vy + rotaPower * (l1 - r1) + rotaStickPower * w; |
echo_piyo | 55:2dd2f161ebaf | 43 | V3 = -vx + vy + rotaPower * (l1 - r1) + rotaStickPower * w; |
echo_piyo | 55:2dd2f161ebaf | 44 | V4 = vx - vy + rotaPower * (l1 - r1) + rotaStickPower * w; |
echo_piyo | 0:bf96e953cdb8 | 45 | |
echo_piyo | 0:bf96e953cdb8 | 46 | } |
echo_piyo | 0:bf96e953cdb8 | 47 | |
echo_piyo | 5:6efda58ff71b | 48 | void boost(){ |
echo_piyo | 5:6efda58ff71b | 49 | if(vy > 0.4){ |
echo_piyo | 10:04f2a82cfd89 | 50 | V1 += boost_A; |
echo_piyo | 10:04f2a82cfd89 | 51 | V2 += -boost_A; |
echo_piyo | 10:04f2a82cfd89 | 52 | V3 += boost_A; |
echo_piyo | 10:04f2a82cfd89 | 53 | V4 += -boost_A; |
echo_piyo | 5:6efda58ff71b | 54 | }else if(vy < -0.4){ |
echo_piyo | 10:04f2a82cfd89 | 55 | V1 += -boost_A; |
echo_piyo | 10:04f2a82cfd89 | 56 | V2 += boost_A; |
echo_piyo | 10:04f2a82cfd89 | 57 | V3 += -boost_A; |
echo_piyo | 10:04f2a82cfd89 | 58 | V4 += boost_A; |
echo_piyo | 5:6efda58ff71b | 59 | } |
echo_piyo | 10:04f2a82cfd89 | 60 | } |
echo_piyo | 10:04f2a82cfd89 | 61 | |
echo_piyo | 10:04f2a82cfd89 | 62 | void boost_forward(){ |
echo_piyo | 10:04f2a82cfd89 | 63 | V1 = -boost_K; |
echo_piyo | 10:04f2a82cfd89 | 64 | V2 = boost_K; |
echo_piyo | 10:04f2a82cfd89 | 65 | V3 = -boost_K; |
echo_piyo | 10:04f2a82cfd89 | 66 | V4 = boost_K; |
echo_piyo | 10:04f2a82cfd89 | 67 | } |
echo_piyo | 10:04f2a82cfd89 | 68 | |
echo_piyo | 10:04f2a82cfd89 | 69 | void boost_back(){ |
echo_piyo | 10:04f2a82cfd89 | 70 | V1 = boost_K; |
echo_piyo | 10:04f2a82cfd89 | 71 | V2 = -boost_K; |
echo_piyo | 10:04f2a82cfd89 | 72 | V3 = boost_K; |
echo_piyo | 10:04f2a82cfd89 | 73 | V4 = -boost_K; |
echo_piyo | 10:04f2a82cfd89 | 74 | } |
echo_piyo | 10:04f2a82cfd89 | 75 | |
echo_piyo | 10:04f2a82cfd89 | 76 | void boost_right(){ |
echo_piyo | 10:04f2a82cfd89 | 77 | V1 = boost_K; |
echo_piyo | 10:04f2a82cfd89 | 78 | V2 = -boost_K; |
echo_piyo | 10:04f2a82cfd89 | 79 | V3 = -boost_K; |
echo_piyo | 10:04f2a82cfd89 | 80 | V4 = boost_K; |
echo_piyo | 10:04f2a82cfd89 | 81 | } |
echo_piyo | 10:04f2a82cfd89 | 82 | |
echo_piyo | 10:04f2a82cfd89 | 83 | void boost_left(){ |
echo_piyo | 10:04f2a82cfd89 | 84 | V1 = -boost_K; |
echo_piyo | 10:04f2a82cfd89 | 85 | V2 = boost_K; |
echo_piyo | 10:04f2a82cfd89 | 86 | V3 = boost_K; |
echo_piyo | 10:04f2a82cfd89 | 87 | V4 = -boost_K; |
echo_piyo | 10:04f2a82cfd89 | 88 | } |
echo_piyo | 10:04f2a82cfd89 | 89 | |
echo_piyo | 5:6efda58ff71b | 90 | |
echo_piyo | 0:bf96e953cdb8 | 91 | void xy_cal(float Vx, float Vy){ |
echo_piyo | 0:bf96e953cdb8 | 92 | V1 = Vx / 2 + Vy / 2; |
echo_piyo | 0:bf96e953cdb8 | 93 | V2 = Vx / 2 - Vy / 2; |
echo_piyo | 0:bf96e953cdb8 | 94 | V3 = -Vx / 2 + Vy / 2; |
echo_piyo | 0:bf96e953cdb8 | 95 | V4 = -Vx / 2 - Vy / 2; |
echo_piyo | 0:bf96e953cdb8 | 96 | } |
echo_piyo | 0:bf96e953cdb8 | 97 | |
echo_piyo | 0:bf96e953cdb8 | 98 | float v1(){ |
echo_piyo | 0:bf96e953cdb8 | 99 | return V1; |
echo_piyo | 0:bf96e953cdb8 | 100 | } |
echo_piyo | 0:bf96e953cdb8 | 101 | |
echo_piyo | 0:bf96e953cdb8 | 102 | float v2(){ |
echo_piyo | 0:bf96e953cdb8 | 103 | return V2; |
echo_piyo | 0:bf96e953cdb8 | 104 | } |
echo_piyo | 0:bf96e953cdb8 | 105 | |
echo_piyo | 0:bf96e953cdb8 | 106 | float v3(){ |
echo_piyo | 0:bf96e953cdb8 | 107 | return V3; |
echo_piyo | 0:bf96e953cdb8 | 108 | } |
echo_piyo | 0:bf96e953cdb8 | 109 | |
echo_piyo | 0:bf96e953cdb8 | 110 | float v4(){ |
echo_piyo | 0:bf96e953cdb8 | 111 | return V4; |
echo_piyo | 0:bf96e953cdb8 | 112 | } |
echo_piyo | 0:bf96e953cdb8 | 113 | |
echo_piyo | 0:bf96e953cdb8 | 114 | float VX(){ |
echo_piyo | 0:bf96e953cdb8 | 115 | return vx; |
echo_piyo | 0:bf96e953cdb8 | 116 | } |
echo_piyo | 0:bf96e953cdb8 | 117 | |
echo_piyo | 0:bf96e953cdb8 | 118 | float VY(){ |
echo_piyo | 0:bf96e953cdb8 | 119 | return vy; |
echo_piyo | 0:bf96e953cdb8 | 120 | } |
echo_piyo | 0:bf96e953cdb8 | 121 | |
echo_piyo | 0:bf96e953cdb8 | 122 | |
echo_piyo | 0:bf96e953cdb8 | 123 | private: |
echo_piyo | 0:bf96e953cdb8 | 124 | float V1, V2, V3, V4, vx, vy; |
echo_piyo | 0:bf96e953cdb8 | 125 | float first_deg, rad; |
echo_piyo | 0:bf96e953cdb8 | 126 | }; |