ライブラリ化を行った後
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
bno055_use.h
- Committer:
- echo_piyo
- Date:
- 2017-06-29
- Revision:
- 2:d5b8f8e62923
- Parent:
- 1:2d878962e6ea
- Child:
- 5:6efda58ff71b
File content as of revision 2:d5b8f8e62923:
/************************************************* [使い方] 1. #include "bno005_lib.h" //BNO055 本家ライブラリ #include "bno005_use.h" //BNO055 データ取得用Class を読み込ませる 2. bno055_use.hの#defineでI2Cピン指定をする 3. main()内での初期設定を行う wait(1); //センサの位置取得待ち時間 .begin(); //センサ起動処理 wait(1); //センサの位置取得待ち時間 .firstRead(); //初期値設定 [Function] .begin() //センサ起動処理 .firstRead() //初期値設定(センサ正面を0度設定) .getYaw() //Yawセンサ値(回転角上限なし) 値取得 (返り値 float) .getYaw180() //Yaw(-180~0~180) 値取得 (返り値 float) .getYaw360() //Yaw(0~360)(時計回り) 値取得 (返り値 float) .getYaw360Rev() //Yaw(0~360)(反時計回り) 値取得 (返り値 float) **************************************************/ //p28 p27 or p9 p10 #define BNO005_SDA p9 #define BNO005_SCL p10 #define timer_dt 0.010 #define ifaceI2C_frequency 200000 I2C ifaceI2C(BNO005_SDA,BNO005_SCL);//BNO055用 I2C宣言 BOARDC_BNO055 sensor1(&ifaceI2C); //BNO055用 ライブラリ使用宣言 class Bno055 { public : Bno055(){ shortdata_yaw = 0; count = 0; data_scEUL = 0; data_yaw = 0; data_firstYaw = 0; data_yaw180 = 0; data_yaw360 = 0; data_yaw360Rev = 0; data_yawRad = 0; } void begin(){ cal_timer.attach(this, &Bno055::getData, timer_dt); ifaceI2C.frequency(ifaceI2C_frequency); sensor1.initialize(true); yaw_state = 1; shortdata_yaw = 0; count = 0; data_scEUL = 0; data_yaw = 0; data_firstYaw = 0; data_yaw180 = 0; data_yaw360 = 0; data_yaw360Rev = 0; data_yawRad = 0; } void firstRead(){ data_scEUL = sensor1.getEulerScale(); shortdata_yaw = sensor1.getEulerDataHeading(); data_firstYaw = (float)shortdata_yaw * data_scEUL; } float getYaw(){ return data_yaw; } float getFirstYaw(){ return data_firstYaw; } float getscEUL(){ return data_scEUL; } float getYaw180(){ //正面0基準変換 if(data_yaw < data_firstYaw){ data_yaw180 = 360.0 + data_yaw - data_firstYaw; }else{ data_yaw180 = data_yaw - data_firstYaw; } //-180~0~180に変換 if(data_yaw180 >= 180){ data_yaw180 = (360 - data_yaw180) * -1; } return data_yaw180; } float getYaw360(){ //正面0基準変換 if(data_yaw < data_firstYaw){ data_yaw360 = 360.0 + data_yaw - data_firstYaw; }else{ data_yaw360 = data_yaw - data_firstYaw; } return data_yaw360; } float getYaw360Rev(){ //正面0基準変換 if(data_yaw < data_firstYaw){ data_yaw360Rev = 360.0 + data_yaw - data_firstYaw; }else{ data_yaw360Rev = data_yaw - data_firstYaw; } //反時計周りに増加 data_yaw360Rev = 360 - data_yaw360Rev; return data_yaw360Rev; } float getYawRad(){ return data_yawRad; } int getYawState(){ return yaw_state; } void calc_yawRad(){ switch(yaw_state){ case 1: if(data_yaw180 >= 90){ yaw_state = 2; }else if(data_yaw180 <= -90){ yaw_state = 3; } data_yawRad = 360.0 * count + data_yaw180; break; case 2: if(data_yaw360 > 270){ yaw_state = 1; count++; }else if(data_yaw360 < 90){ yaw_state = 1; } data_yawRad = 360.0 * count + data_yaw360; break; case 3: if(data_yaw360 < 90){ yaw_state = 1; count--; }else if(data_yaw360 > 270){ yaw_state = 1; } data_yawRad = 360.0 * count - data_yaw360Rev; break; } } int getCount(){ return count; } private : Ticker cal_timer; short BNO055_dataBox[12]; short shortdata_yaw; int count; int yaw_state; float data_scAcc, data_scMag, data_scGyro, data_scEUL, data_scTemp; float data_ax, data_ay, data_az, data_mx, data_my, data_mz, data_gx, data_gy, data_gz, data_yaw, data_roll, data_pitch, data_temp, data_firstYaw, data_yaw180, data_yaw360, data_yaw360Rev ,data_yawRad; void getData(){ shortdata_yaw = sensor1.getEulerDataHeading(); data_yaw = (float)shortdata_yaw * data_scEUL; getYaw180(); getYaw360(); getYaw360Rev(); calc_yawRad(); } };