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@60:07d7bde726f9, 2017-09-20 (annotated)

Committer:
echo_piyo
Date:
Wed Sep 20 04:04:27 2017 +0000
Revision:
60:07d7bde726f9
Parent:
56:a7bd860b85b6 
(????????)

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 60:07d7bde726f9 24 #define rotaStickPower 0.4
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 };