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@0:bf96e953cdb8, 2017-06-26 (annotated)

Committer:
echo_piyo
Date:
Mon Jun 26 09:59:14 2017 +0000
Revision:
0:bf96e953cdb8
Child:
5:6efda58ff71b
????????????????

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