kusano kiyoshige / Mbed 2 deprecated 17robo_tokyo_kaede

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

mecanum.h@52:f5ae47e683fa, 2017-09-14 (annotated)

Committer:
echo_piyo
Date:
Thu Sep 14 11:56:51 2017 +0000
Revision:
52:f5ae47e683fa
Parent:
41:a4e37077833b
Child:
55:2dd2f161ebaf
(?)????????????????; ??????????????1????????????????????????????????????

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