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

headerfile_use/bno055_use.h@66:1664ee92539d, 2017-09-24 (annotated)

Committer:
echo_piyo
Date:
Sun Sep 24 05:25:03 2017 +0000
Revision:
66:1664ee92539d
Parent:
bno055_use.h@46:4af9df24b94c 
????2??

Who changed what in which revision?

UserRevisionLine numberNew contents of line
echo_piyo 0:bf96e953cdb8 1 /*************************************************
echo_piyo 0:bf96e953cdb8 2 [使い方]
echo_piyo 0:bf96e953cdb8 3 1. #include "bno005_lib.h" //BNO055 本家ライブラリ
echo_piyo 0:bf96e953cdb8 4 #include "bno005_use.h" //BNO055 データ取得用Class
echo_piyo 0:bf96e953cdb8 5 を読み込ませる
echo_piyo 0:bf96e953cdb8 6 2. bno055_use.hの#defineでI2Cピン指定をする
echo_piyo 0:bf96e953cdb8 7 3. main()内での初期設定を行う
echo_piyo 1:2d878962e6ea 8 wait(1); //センサの位置取得待ち時間
echo_piyo 0:bf96e953cdb8 9 .begin(); //センサ起動処理
echo_piyo 0:bf96e953cdb8 10 wait(1); //センサの位置取得待ち時間
echo_piyo 0:bf96e953cdb8 11 .firstRead(); //初期値設定
echo_piyo 0:bf96e953cdb8 12
echo_piyo 0:bf96e953cdb8 13 [Function]
echo_piyo 0:bf96e953cdb8 14 .begin() //センサ起動処理
echo_piyo 0:bf96e953cdb8 15 .firstRead() //初期値設定(センサ正面を0度設定)
echo_piyo 0:bf96e953cdb8 16 .getYaw() //Yawセンサ値(回転角上限なし) 値取得 (返り値 float)
echo_piyo 0:bf96e953cdb8 17 .getYaw180() //Yaw(-180~0~180) 値取得 (返り値 float)
echo_piyo 0:bf96e953cdb8 18 .getYaw360() //Yaw(0~360)(時計回り) 値取得 (返り値 float)
echo_piyo 0:bf96e953cdb8 19 .getYaw360Rev() //Yaw(0~360)(反時計回り) 値取得 (返り値 float)
echo_piyo 46:4af9df24b94c 20 .init()
echo_piyo 46:4af9df24b94c 21 .yaw_origin()
echo_piyo 0:bf96e953cdb8 22
echo_piyo 0:bf96e953cdb8 23 **************************************************/
echo_piyo 0:bf96e953cdb8 24 //p28 p27 or p9 p10
echo_piyo 0:bf96e953cdb8 25
echo_piyo 0:bf96e953cdb8 26 #define BNO005_SDA p9
echo_piyo 0:bf96e953cdb8 27 #define BNO005_SCL p10
echo_piyo 1:2d878962e6ea 28 #define timer_dt 0.010
echo_piyo 0:bf96e953cdb8 29 #define ifaceI2C_frequency 200000
echo_piyo 0:bf96e953cdb8 30
echo_piyo 0:bf96e953cdb8 31 I2C ifaceI2C(BNO005_SDA,BNO005_SCL);//BNO055用 I2C宣言
echo_piyo 0:bf96e953cdb8 32 BOARDC_BNO055 sensor1(&ifaceI2C); //BNO055用 ライブラリ使用宣言
echo_piyo 0:bf96e953cdb8 33
echo_piyo 0:bf96e953cdb8 34 class Bno055 {
echo_piyo 0:bf96e953cdb8 35 public :
echo_piyo 0:bf96e953cdb8 36 Bno055(){
echo_piyo 0:bf96e953cdb8 37 shortdata_yaw = 0;
echo_piyo 0:bf96e953cdb8 38 count = 0;
echo_piyo 0:bf96e953cdb8 39 data_scEUL = 0;
echo_piyo 0:bf96e953cdb8 40 data_yaw = 0;
echo_piyo 0:bf96e953cdb8 41 data_firstYaw = 0;
echo_piyo 0:bf96e953cdb8 42 data_yaw180 = 0;
echo_piyo 0:bf96e953cdb8 43 data_yaw360 = 0;
echo_piyo 0:bf96e953cdb8 44 data_yaw360Rev = 0;
echo_piyo 0:bf96e953cdb8 45 data_yawRad = 0;
echo_piyo 0:bf96e953cdb8 46 }
echo_piyo 0:bf96e953cdb8 47
echo_piyo 0:bf96e953cdb8 48 void begin(){
echo_piyo 0:bf96e953cdb8 49 cal_timer.attach(this, &Bno055::getData, timer_dt);
echo_piyo 0:bf96e953cdb8 50 ifaceI2C.frequency(ifaceI2C_frequency);
echo_piyo 0:bf96e953cdb8 51 sensor1.initialize(true);
echo_piyo 46:4af9df24b94c 52 init();
echo_piyo 46:4af9df24b94c 53 }
echo_piyo 46:4af9df24b94c 54
echo_piyo 46:4af9df24b94c 55 void init(){
echo_piyo 0:bf96e953cdb8 56 yaw_state = 1;
echo_piyo 2:d5b8f8e62923 57 shortdata_yaw = 0;
echo_piyo 2:d5b8f8e62923 58 count = 0;
echo_piyo 2:d5b8f8e62923 59 data_scEUL = 0;
echo_piyo 2:d5b8f8e62923 60 data_yaw = 0;
echo_piyo 2:d5b8f8e62923 61 data_firstYaw = 0;
echo_piyo 2:d5b8f8e62923 62 data_yaw180 = 0;
echo_piyo 2:d5b8f8e62923 63 data_yaw360 = 0;
echo_piyo 2:d5b8f8e62923 64 data_yaw360Rev = 0;
echo_piyo 46:4af9df24b94c 65 data_yawRad = 0;
echo_piyo 0:bf96e953cdb8 66 }
echo_piyo 0:bf96e953cdb8 67
echo_piyo 0:bf96e953cdb8 68 void firstRead(){
echo_piyo 0:bf96e953cdb8 69 data_scEUL = sensor1.getEulerScale();
echo_piyo 0:bf96e953cdb8 70 shortdata_yaw = sensor1.getEulerDataHeading();
echo_piyo 0:bf96e953cdb8 71 data_firstYaw = (float)shortdata_yaw * data_scEUL;
echo_piyo 0:bf96e953cdb8 72 }
echo_piyo 0:bf96e953cdb8 73
echo_piyo 46:4af9df24b94c 74 void yaw_origin(){
echo_piyo 46:4af9df24b94c 75 data_firstYaw = data_yaw;
echo_piyo 46:4af9df24b94c 76 }
echo_piyo 46:4af9df24b94c 77
echo_piyo 0:bf96e953cdb8 78 float getYaw(){
echo_piyo 0:bf96e953cdb8 79 return data_yaw;
echo_piyo 0:bf96e953cdb8 80 }
echo_piyo 0:bf96e953cdb8 81
echo_piyo 0:bf96e953cdb8 82 float getFirstYaw(){
echo_piyo 0:bf96e953cdb8 83 return data_firstYaw;
echo_piyo 0:bf96e953cdb8 84 }
echo_piyo 0:bf96e953cdb8 85
echo_piyo 0:bf96e953cdb8 86 float getscEUL(){
echo_piyo 0:bf96e953cdb8 87 return data_scEUL;
echo_piyo 0:bf96e953cdb8 88 }
echo_piyo 0:bf96e953cdb8 89
echo_piyo 0:bf96e953cdb8 90 float getYaw180(){
echo_piyo 0:bf96e953cdb8 91 //正面0基準変換
echo_piyo 0:bf96e953cdb8 92 if(data_yaw < data_firstYaw){
echo_piyo 0:bf96e953cdb8 93 data_yaw180 = 360.0 + data_yaw - data_firstYaw;
echo_piyo 0:bf96e953cdb8 94 }else{
echo_piyo 0:bf96e953cdb8 95 data_yaw180 = data_yaw - data_firstYaw;
echo_piyo 0:bf96e953cdb8 96 }
echo_piyo 0:bf96e953cdb8 97 //-180~0~180に変換
echo_piyo 0:bf96e953cdb8 98 if(data_yaw180 >= 180){
echo_piyo 0:bf96e953cdb8 99 data_yaw180 = (360 - data_yaw180) * -1;
echo_piyo 0:bf96e953cdb8 100 }
echo_piyo 0:bf96e953cdb8 101 return data_yaw180;
echo_piyo 0:bf96e953cdb8 102 }
echo_piyo 0:bf96e953cdb8 103
echo_piyo 0:bf96e953cdb8 104 float getYaw360(){
echo_piyo 0:bf96e953cdb8 105 //正面0基準変換
echo_piyo 0:bf96e953cdb8 106 if(data_yaw < data_firstYaw){
echo_piyo 0:bf96e953cdb8 107 data_yaw360 = 360.0 + data_yaw - data_firstYaw;
echo_piyo 0:bf96e953cdb8 108 }else{
echo_piyo 0:bf96e953cdb8 109 data_yaw360 = data_yaw - data_firstYaw;
echo_piyo 0:bf96e953cdb8 110 }
echo_piyo 0:bf96e953cdb8 111 return data_yaw360;
echo_piyo 0:bf96e953cdb8 112 }
echo_piyo 0:bf96e953cdb8 113
echo_piyo 0:bf96e953cdb8 114 float getYaw360Rev(){
echo_piyo 0:bf96e953cdb8 115 //正面0基準変換
echo_piyo 0:bf96e953cdb8 116 if(data_yaw < data_firstYaw){
echo_piyo 0:bf96e953cdb8 117 data_yaw360Rev = 360.0 + data_yaw - data_firstYaw;
echo_piyo 0:bf96e953cdb8 118 }else{
echo_piyo 0:bf96e953cdb8 119 data_yaw360Rev = data_yaw - data_firstYaw;
echo_piyo 0:bf96e953cdb8 120 }
echo_piyo 0:bf96e953cdb8 121 //反時計周りに増加
echo_piyo 0:bf96e953cdb8 122 data_yaw360Rev = 360 - data_yaw360Rev;
echo_piyo 0:bf96e953cdb8 123 return data_yaw360Rev;
echo_piyo 0:bf96e953cdb8 124 }
echo_piyo 0:bf96e953cdb8 125
echo_piyo 0:bf96e953cdb8 126 float getYawRad(){
echo_piyo 0:bf96e953cdb8 127 return data_yawRad;
echo_piyo 0:bf96e953cdb8 128 }
echo_piyo 0:bf96e953cdb8 129
echo_piyo 0:bf96e953cdb8 130 int getYawState(){
echo_piyo 0:bf96e953cdb8 131 return yaw_state;
echo_piyo 0:bf96e953cdb8 132 }
echo_piyo 0:bf96e953cdb8 133
echo_piyo 0:bf96e953cdb8 134 void calc_yawRad(){
echo_piyo 0:bf96e953cdb8 135 switch(yaw_state){
echo_piyo 0:bf96e953cdb8 136
echo_piyo 0:bf96e953cdb8 137 case 1:
echo_piyo 0:bf96e953cdb8 138 if(data_yaw180 >= 90){
echo_piyo 0:bf96e953cdb8 139 yaw_state = 2;
echo_piyo 0:bf96e953cdb8 140 }else if(data_yaw180 <= -90){
echo_piyo 0:bf96e953cdb8 141 yaw_state = 3;
echo_piyo 0:bf96e953cdb8 142 }
echo_piyo 0:bf96e953cdb8 143 data_yawRad = 360.0 * count + data_yaw180;
echo_piyo 0:bf96e953cdb8 144 break;
echo_piyo 0:bf96e953cdb8 145
echo_piyo 0:bf96e953cdb8 146 case 2:
echo_piyo 0:bf96e953cdb8 147 if(data_yaw360 > 270){
echo_piyo 0:bf96e953cdb8 148 yaw_state = 1;
echo_piyo 0:bf96e953cdb8 149 count++;
echo_piyo 0:bf96e953cdb8 150 }else if(data_yaw360 < 90){
echo_piyo 0:bf96e953cdb8 151 yaw_state = 1;
echo_piyo 0:bf96e953cdb8 152 }
echo_piyo 0:bf96e953cdb8 153 data_yawRad = 360.0 * count + data_yaw360;
echo_piyo 0:bf96e953cdb8 154 break;
echo_piyo 0:bf96e953cdb8 155
echo_piyo 0:bf96e953cdb8 156 case 3:
echo_piyo 0:bf96e953cdb8 157 if(data_yaw360 < 90){
echo_piyo 0:bf96e953cdb8 158 yaw_state = 1;
echo_piyo 0:bf96e953cdb8 159 count--;
echo_piyo 0:bf96e953cdb8 160 }else if(data_yaw360 > 270){
echo_piyo 0:bf96e953cdb8 161 yaw_state = 1;
echo_piyo 0:bf96e953cdb8 162 }
echo_piyo 0:bf96e953cdb8 163 data_yawRad = 360.0 * count - data_yaw360Rev;
echo_piyo 0:bf96e953cdb8 164 break;
echo_piyo 0:bf96e953cdb8 165 }
echo_piyo 0:bf96e953cdb8 166 }
echo_piyo 0:bf96e953cdb8 167
echo_piyo 0:bf96e953cdb8 168 int getCount(){
echo_piyo 0:bf96e953cdb8 169 return count;
echo_piyo 0:bf96e953cdb8 170 }
echo_piyo 0:bf96e953cdb8 171
echo_piyo 5:6efda58ff71b 172 int getYaw_state(){
echo_piyo 5:6efda58ff71b 173 return yaw_state;
echo_piyo 5:6efda58ff71b 174 }
echo_piyo 5:6efda58ff71b 175
echo_piyo 0:bf96e953cdb8 176
echo_piyo 0:bf96e953cdb8 177 private :
echo_piyo 0:bf96e953cdb8 178 Ticker cal_timer;
echo_piyo 0:bf96e953cdb8 179
echo_piyo 0:bf96e953cdb8 180 short BNO055_dataBox[12];
echo_piyo 0:bf96e953cdb8 181 short shortdata_yaw;
echo_piyo 0:bf96e953cdb8 182 int count;
echo_piyo 0:bf96e953cdb8 183 int yaw_state;
echo_piyo 0:bf96e953cdb8 184 float data_scAcc, data_scMag, data_scGyro, data_scEUL, data_scTemp;
echo_piyo 0:bf96e953cdb8 185 float data_ax, data_ay, data_az,
echo_piyo 0:bf96e953cdb8 186 data_mx, data_my, data_mz,
echo_piyo 0:bf96e953cdb8 187 data_gx, data_gy, data_gz,
echo_piyo 0:bf96e953cdb8 188 data_yaw, data_roll, data_pitch,
echo_piyo 0:bf96e953cdb8 189 data_temp,
echo_piyo 0:bf96e953cdb8 190 data_firstYaw, data_yaw180, data_yaw360, data_yaw360Rev
echo_piyo 0:bf96e953cdb8 191 ,data_yawRad;
echo_piyo 0:bf96e953cdb8 192
echo_piyo 0:bf96e953cdb8 193 void getData(){
echo_piyo 0:bf96e953cdb8 194 shortdata_yaw = sensor1.getEulerDataHeading();
echo_piyo 0:bf96e953cdb8 195 data_yaw = (float)shortdata_yaw * data_scEUL;
echo_piyo 0:bf96e953cdb8 196 getYaw180();
echo_piyo 0:bf96e953cdb8 197 getYaw360();
echo_piyo 0:bf96e953cdb8 198 getYaw360Rev();
echo_piyo 0:bf96e953cdb8 199 calc_yawRad();
echo_piyo 0:bf96e953cdb8 200 }
echo_piyo 0:bf96e953cdb8 201 };