![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ライブラリ化を行った後
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
Diff: headerfile_use/bno055_use.h
- Revision:
- 66:1664ee92539d
- Parent:
- 46:4af9df24b94c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/headerfile_use/bno055_use.h Sun Sep 24 05:25:03 2017 +0000 @@ -0,0 +1,201 @@ +/************************************************* +[使い方] +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) + .init() + .yaw_origin() + +**************************************************/ +//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); + init(); + } + + void init(){ + 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; + } + + void yaw_origin(){ + data_firstYaw = data_yaw; + } + + 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; + } + + int getYaw_state(){ + return yaw_state; + } + + +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(); + } +}; \ No newline at end of file