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.
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
bno055_use.h
00001 /************************************************* 00002 [使い方] 00003 1. #include "bno005_lib.h" //BNO055 本家ライブラリ 00004 #include "bno005_use.h" //BNO055 データ取得用Class 00005 を読み込ませる 00006 2. bno055_use.hの#defineでI2Cピン指定をする 00007 3. main()内での初期設定を行う 00008 wait(1); //センサの位置取得待ち時間 00009 .begin(); //センサ起動処理 00010 wait(1); //センサの位置取得待ち時間 00011 .firstRead(); //初期値設定 00012 00013 [Function] 00014 .begin() //センサ起動処理 00015 .firstRead() //初期値設定(センサ正面を0度設定) 00016 .getYaw() //Yawセンサ値(回転角上限なし) 値取得 (返り値 float) 00017 .getYaw180() //Yaw(-180~0~180) 値取得 (返り値 float) 00018 .getYaw360() //Yaw(0~360)(時計回り) 値取得 (返り値 float) 00019 .getYaw360Rev() //Yaw(0~360)(反時計回り) 値取得 (返り値 float) 00020 .init() 00021 .yaw_origin() 00022 00023 **************************************************/ 00024 //p28 p27 or p9 p10 00025 00026 #define BNO005_SDA p9 00027 #define BNO005_SCL p10 00028 #define timer_dt 0.010 00029 #define ifaceI2C_frequency 200000 00030 00031 I2C ifaceI2C(BNO005_SDA,BNO005_SCL);//BNO055用 I2C宣言 00032 BOARDC_BNO055 sensor1(&ifaceI2C); //BNO055用 ライブラリ使用宣言 00033 00034 class Bno055 { 00035 public : 00036 Bno055(){ 00037 shortdata_yaw = 0; 00038 count = 0; 00039 data_scEUL = 0; 00040 data_yaw = 0; 00041 data_firstYaw = 0; 00042 data_yaw180 = 0; 00043 data_yaw360 = 0; 00044 data_yaw360Rev = 0; 00045 data_yawRad = 0; 00046 } 00047 00048 void begin(){ 00049 cal_timer.attach(this, &Bno055::getData, timer_dt); 00050 ifaceI2C.frequency(ifaceI2C_frequency); 00051 sensor1.initialize(true); 00052 init(); 00053 } 00054 00055 void init(){ 00056 yaw_state = 1; 00057 shortdata_yaw = 0; 00058 count = 0; 00059 data_scEUL = 0; 00060 data_yaw = 0; 00061 data_firstYaw = 0; 00062 data_yaw180 = 0; 00063 data_yaw360 = 0; 00064 data_yaw360Rev = 0; 00065 data_yawRad = 0; 00066 } 00067 00068 void firstRead(){ 00069 data_scEUL = sensor1.getEulerScale(); 00070 shortdata_yaw = sensor1.getEulerDataHeading(); 00071 data_firstYaw = (float)shortdata_yaw * data_scEUL; 00072 } 00073 00074 void yaw_origin(){ 00075 data_firstYaw = data_yaw; 00076 } 00077 00078 float getYaw(){ 00079 return data_yaw; 00080 } 00081 00082 float getFirstYaw(){ 00083 return data_firstYaw; 00084 } 00085 00086 float getscEUL(){ 00087 return data_scEUL; 00088 } 00089 00090 float getYaw180(){ 00091 //正面0基準変換 00092 if(data_yaw < data_firstYaw){ 00093 data_yaw180 = 360.0 + data_yaw - data_firstYaw; 00094 }else{ 00095 data_yaw180 = data_yaw - data_firstYaw; 00096 } 00097 //-180~0~180に変換 00098 if(data_yaw180 >= 180){ 00099 data_yaw180 = (360 - data_yaw180) * -1; 00100 } 00101 return data_yaw180; 00102 } 00103 00104 float getYaw360(){ 00105 //正面0基準変換 00106 if(data_yaw < data_firstYaw){ 00107 data_yaw360 = 360.0 + data_yaw - data_firstYaw; 00108 }else{ 00109 data_yaw360 = data_yaw - data_firstYaw; 00110 } 00111 return data_yaw360; 00112 } 00113 00114 float getYaw360Rev(){ 00115 //正面0基準変換 00116 if(data_yaw < data_firstYaw){ 00117 data_yaw360Rev = 360.0 + data_yaw - data_firstYaw; 00118 }else{ 00119 data_yaw360Rev = data_yaw - data_firstYaw; 00120 } 00121 //反時計周りに増加 00122 data_yaw360Rev = 360 - data_yaw360Rev; 00123 return data_yaw360Rev; 00124 } 00125 00126 float getYawRad(){ 00127 return data_yawRad; 00128 } 00129 00130 int getYawState(){ 00131 return yaw_state; 00132 } 00133 00134 void calc_yawRad(){ 00135 switch(yaw_state){ 00136 00137 case 1: 00138 if(data_yaw180 >= 90){ 00139 yaw_state = 2; 00140 }else if(data_yaw180 <= -90){ 00141 yaw_state = 3; 00142 } 00143 data_yawRad = 360.0 * count + data_yaw180; 00144 break; 00145 00146 case 2: 00147 if(data_yaw360 > 270){ 00148 yaw_state = 1; 00149 count++; 00150 }else if(data_yaw360 < 90){ 00151 yaw_state = 1; 00152 } 00153 data_yawRad = 360.0 * count + data_yaw360; 00154 break; 00155 00156 case 3: 00157 if(data_yaw360 < 90){ 00158 yaw_state = 1; 00159 count--; 00160 }else if(data_yaw360 > 270){ 00161 yaw_state = 1; 00162 } 00163 data_yawRad = 360.0 * count - data_yaw360Rev; 00164 break; 00165 } 00166 } 00167 00168 int getCount(){ 00169 return count; 00170 } 00171 00172 int getYaw_state(){ 00173 return yaw_state; 00174 } 00175 00176 00177 private : 00178 Ticker cal_timer; 00179 00180 short BNO055_dataBox[12]; 00181 short shortdata_yaw; 00182 int count; 00183 int yaw_state; 00184 float data_scAcc, data_scMag, data_scGyro, data_scEUL, data_scTemp; 00185 float data_ax, data_ay, data_az, 00186 data_mx, data_my, data_mz, 00187 data_gx, data_gy, data_gz, 00188 data_yaw, data_roll, data_pitch, 00189 data_temp, 00190 data_firstYaw, data_yaw180, data_yaw360, data_yaw360Rev 00191 ,data_yawRad; 00192 00193 void getData(){ 00194 shortdata_yaw = sensor1.getEulerDataHeading(); 00195 data_yaw = (float)shortdata_yaw * data_scEUL; 00196 getYaw180(); 00197 getYaw360(); 00198 getYaw360Rev(); 00199 calc_yawRad(); 00200 } 00201 };
Generated on Mon Aug 8 2022 20:27:09 by
1.7.2