Kobayashi Takumi / BNO055_mbed

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

bno055_use.h@4:8d3cc21f184e, 2019-05-31 (annotated)

Committer:
YutaTogashi
Date:
Fri May 31 13:35:05 2019 +0000
Revision:
4:8d3cc21f184e
Parent:
3:5b036c235d4d
Child:
6:23e029bd9c9b
20190531

Who changed what in which revision?

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