![](/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
bno055_use.h@5:6efda58ff71b, 2017-06-30 (annotated)
- Committer:
- echo_piyo
- Date:
- Fri Jun 30 05:40:02 2017 +0000
- Revision:
- 5:6efda58ff71b
- Parent:
- 2:d5b8f8e62923
- Child:
- 6:acefbf83ffe1
- Child:
- 46:4af9df24b94c
?????????????????????????
Who changed what in which revision?
User | Revision | Line number | New 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 | 0:bf96e953cdb8 | 20 | |
echo_piyo | 0:bf96e953cdb8 | 21 | **************************************************/ |
echo_piyo | 0:bf96e953cdb8 | 22 | //p28 p27 or p9 p10 |
echo_piyo | 0:bf96e953cdb8 | 23 | |
echo_piyo | 0:bf96e953cdb8 | 24 | #define BNO005_SDA p9 |
echo_piyo | 0:bf96e953cdb8 | 25 | #define BNO005_SCL p10 |
echo_piyo | 1:2d878962e6ea | 26 | #define timer_dt 0.010 |
echo_piyo | 0:bf96e953cdb8 | 27 | #define ifaceI2C_frequency 200000 |
echo_piyo | 0:bf96e953cdb8 | 28 | |
echo_piyo | 0:bf96e953cdb8 | 29 | I2C ifaceI2C(BNO005_SDA,BNO005_SCL);//BNO055用 I2C宣言 |
echo_piyo | 0:bf96e953cdb8 | 30 | BOARDC_BNO055 sensor1(&ifaceI2C); //BNO055用 ライブラリ使用宣言 |
echo_piyo | 0:bf96e953cdb8 | 31 | |
echo_piyo | 0:bf96e953cdb8 | 32 | class Bno055 { |
echo_piyo | 0:bf96e953cdb8 | 33 | public : |
echo_piyo | 0:bf96e953cdb8 | 34 | Bno055(){ |
echo_piyo | 0:bf96e953cdb8 | 35 | shortdata_yaw = 0; |
echo_piyo | 0:bf96e953cdb8 | 36 | count = 0; |
echo_piyo | 0:bf96e953cdb8 | 37 | data_scEUL = 0; |
echo_piyo | 0:bf96e953cdb8 | 38 | data_yaw = 0; |
echo_piyo | 0:bf96e953cdb8 | 39 | data_firstYaw = 0; |
echo_piyo | 0:bf96e953cdb8 | 40 | data_yaw180 = 0; |
echo_piyo | 0:bf96e953cdb8 | 41 | data_yaw360 = 0; |
echo_piyo | 0:bf96e953cdb8 | 42 | data_yaw360Rev = 0; |
echo_piyo | 0:bf96e953cdb8 | 43 | data_yawRad = 0; |
echo_piyo | 0:bf96e953cdb8 | 44 | } |
echo_piyo | 0:bf96e953cdb8 | 45 | |
echo_piyo | 0:bf96e953cdb8 | 46 | void begin(){ |
echo_piyo | 0:bf96e953cdb8 | 47 | cal_timer.attach(this, &Bno055::getData, timer_dt); |
echo_piyo | 0:bf96e953cdb8 | 48 | ifaceI2C.frequency(ifaceI2C_frequency); |
echo_piyo | 0:bf96e953cdb8 | 49 | sensor1.initialize(true); |
echo_piyo | 0:bf96e953cdb8 | 50 | yaw_state = 1; |
echo_piyo | 2:d5b8f8e62923 | 51 | shortdata_yaw = 0; |
echo_piyo | 2:d5b8f8e62923 | 52 | count = 0; |
echo_piyo | 2:d5b8f8e62923 | 53 | data_scEUL = 0; |
echo_piyo | 2:d5b8f8e62923 | 54 | data_yaw = 0; |
echo_piyo | 2:d5b8f8e62923 | 55 | data_firstYaw = 0; |
echo_piyo | 2:d5b8f8e62923 | 56 | data_yaw180 = 0; |
echo_piyo | 2:d5b8f8e62923 | 57 | data_yaw360 = 0; |
echo_piyo | 2:d5b8f8e62923 | 58 | data_yaw360Rev = 0; |
echo_piyo | 2:d5b8f8e62923 | 59 | data_yawRad = 0; |
echo_piyo | 0:bf96e953cdb8 | 60 | } |
echo_piyo | 0:bf96e953cdb8 | 61 | |
echo_piyo | 0:bf96e953cdb8 | 62 | void firstRead(){ |
echo_piyo | 0:bf96e953cdb8 | 63 | data_scEUL = sensor1.getEulerScale(); |
echo_piyo | 0:bf96e953cdb8 | 64 | shortdata_yaw = sensor1.getEulerDataHeading(); |
echo_piyo | 0:bf96e953cdb8 | 65 | data_firstYaw = (float)shortdata_yaw * data_scEUL; |
echo_piyo | 0:bf96e953cdb8 | 66 | } |
echo_piyo | 0:bf96e953cdb8 | 67 | |
echo_piyo | 0:bf96e953cdb8 | 68 | float getYaw(){ |
echo_piyo | 0:bf96e953cdb8 | 69 | return data_yaw; |
echo_piyo | 0:bf96e953cdb8 | 70 | } |
echo_piyo | 0:bf96e953cdb8 | 71 | |
echo_piyo | 0:bf96e953cdb8 | 72 | float getFirstYaw(){ |
echo_piyo | 0:bf96e953cdb8 | 73 | return data_firstYaw; |
echo_piyo | 0:bf96e953cdb8 | 74 | } |
echo_piyo | 0:bf96e953cdb8 | 75 | |
echo_piyo | 0:bf96e953cdb8 | 76 | float getscEUL(){ |
echo_piyo | 0:bf96e953cdb8 | 77 | return data_scEUL; |
echo_piyo | 0:bf96e953cdb8 | 78 | } |
echo_piyo | 0:bf96e953cdb8 | 79 | |
echo_piyo | 0:bf96e953cdb8 | 80 | float getYaw180(){ |
echo_piyo | 0:bf96e953cdb8 | 81 | //正面0基準変換 |
echo_piyo | 0:bf96e953cdb8 | 82 | if(data_yaw < data_firstYaw){ |
echo_piyo | 0:bf96e953cdb8 | 83 | data_yaw180 = 360.0 + data_yaw - data_firstYaw; |
echo_piyo | 0:bf96e953cdb8 | 84 | }else{ |
echo_piyo | 0:bf96e953cdb8 | 85 | data_yaw180 = data_yaw - data_firstYaw; |
echo_piyo | 0:bf96e953cdb8 | 86 | } |
echo_piyo | 0:bf96e953cdb8 | 87 | //-180~0~180に変換 |
echo_piyo | 0:bf96e953cdb8 | 88 | if(data_yaw180 >= 180){ |
echo_piyo | 0:bf96e953cdb8 | 89 | data_yaw180 = (360 - data_yaw180) * -1; |
echo_piyo | 0:bf96e953cdb8 | 90 | } |
echo_piyo | 0:bf96e953cdb8 | 91 | return data_yaw180; |
echo_piyo | 0:bf96e953cdb8 | 92 | } |
echo_piyo | 0:bf96e953cdb8 | 93 | |
echo_piyo | 0:bf96e953cdb8 | 94 | float getYaw360(){ |
echo_piyo | 0:bf96e953cdb8 | 95 | //正面0基準変換 |
echo_piyo | 0:bf96e953cdb8 | 96 | if(data_yaw < data_firstYaw){ |
echo_piyo | 0:bf96e953cdb8 | 97 | data_yaw360 = 360.0 + data_yaw - data_firstYaw; |
echo_piyo | 0:bf96e953cdb8 | 98 | }else{ |
echo_piyo | 0:bf96e953cdb8 | 99 | data_yaw360 = data_yaw - data_firstYaw; |
echo_piyo | 0:bf96e953cdb8 | 100 | } |
echo_piyo | 0:bf96e953cdb8 | 101 | return data_yaw360; |
echo_piyo | 0:bf96e953cdb8 | 102 | } |
echo_piyo | 0:bf96e953cdb8 | 103 | |
echo_piyo | 0:bf96e953cdb8 | 104 | float getYaw360Rev(){ |
echo_piyo | 0:bf96e953cdb8 | 105 | //正面0基準変換 |
echo_piyo | 0:bf96e953cdb8 | 106 | if(data_yaw < data_firstYaw){ |
echo_piyo | 0:bf96e953cdb8 | 107 | data_yaw360Rev = 360.0 + data_yaw - data_firstYaw; |
echo_piyo | 0:bf96e953cdb8 | 108 | }else{ |
echo_piyo | 0:bf96e953cdb8 | 109 | data_yaw360Rev = data_yaw - data_firstYaw; |
echo_piyo | 0:bf96e953cdb8 | 110 | } |
echo_piyo | 0:bf96e953cdb8 | 111 | //反時計周りに増加 |
echo_piyo | 0:bf96e953cdb8 | 112 | data_yaw360Rev = 360 - data_yaw360Rev; |
echo_piyo | 0:bf96e953cdb8 | 113 | return data_yaw360Rev; |
echo_piyo | 0:bf96e953cdb8 | 114 | } |
echo_piyo | 0:bf96e953cdb8 | 115 | |
echo_piyo | 0:bf96e953cdb8 | 116 | float getYawRad(){ |
echo_piyo | 0:bf96e953cdb8 | 117 | return data_yawRad; |
echo_piyo | 0:bf96e953cdb8 | 118 | } |
echo_piyo | 0:bf96e953cdb8 | 119 | |
echo_piyo | 0:bf96e953cdb8 | 120 | int getYawState(){ |
echo_piyo | 0:bf96e953cdb8 | 121 | return yaw_state; |
echo_piyo | 0:bf96e953cdb8 | 122 | } |
echo_piyo | 0:bf96e953cdb8 | 123 | |
echo_piyo | 0:bf96e953cdb8 | 124 | void calc_yawRad(){ |
echo_piyo | 0:bf96e953cdb8 | 125 | switch(yaw_state){ |
echo_piyo | 0:bf96e953cdb8 | 126 | |
echo_piyo | 0:bf96e953cdb8 | 127 | case 1: |
echo_piyo | 0:bf96e953cdb8 | 128 | if(data_yaw180 >= 90){ |
echo_piyo | 0:bf96e953cdb8 | 129 | yaw_state = 2; |
echo_piyo | 0:bf96e953cdb8 | 130 | }else if(data_yaw180 <= -90){ |
echo_piyo | 0:bf96e953cdb8 | 131 | yaw_state = 3; |
echo_piyo | 0:bf96e953cdb8 | 132 | } |
echo_piyo | 0:bf96e953cdb8 | 133 | data_yawRad = 360.0 * count + data_yaw180; |
echo_piyo | 0:bf96e953cdb8 | 134 | break; |
echo_piyo | 0:bf96e953cdb8 | 135 | |
echo_piyo | 0:bf96e953cdb8 | 136 | case 2: |
echo_piyo | 0:bf96e953cdb8 | 137 | if(data_yaw360 > 270){ |
echo_piyo | 0:bf96e953cdb8 | 138 | yaw_state = 1; |
echo_piyo | 0:bf96e953cdb8 | 139 | count++; |
echo_piyo | 0:bf96e953cdb8 | 140 | }else if(data_yaw360 < 90){ |
echo_piyo | 0:bf96e953cdb8 | 141 | yaw_state = 1; |
echo_piyo | 0:bf96e953cdb8 | 142 | } |
echo_piyo | 0:bf96e953cdb8 | 143 | data_yawRad = 360.0 * count + data_yaw360; |
echo_piyo | 0:bf96e953cdb8 | 144 | break; |
echo_piyo | 0:bf96e953cdb8 | 145 | |
echo_piyo | 0:bf96e953cdb8 | 146 | case 3: |
echo_piyo | 0:bf96e953cdb8 | 147 | if(data_yaw360 < 90){ |
echo_piyo | 0:bf96e953cdb8 | 148 | yaw_state = 1; |
echo_piyo | 0:bf96e953cdb8 | 149 | count--; |
echo_piyo | 0:bf96e953cdb8 | 150 | }else if(data_yaw360 > 270){ |
echo_piyo | 0:bf96e953cdb8 | 151 | yaw_state = 1; |
echo_piyo | 0:bf96e953cdb8 | 152 | } |
echo_piyo | 0:bf96e953cdb8 | 153 | data_yawRad = 360.0 * count - data_yaw360Rev; |
echo_piyo | 0:bf96e953cdb8 | 154 | break; |
echo_piyo | 0:bf96e953cdb8 | 155 | } |
echo_piyo | 0:bf96e953cdb8 | 156 | } |
echo_piyo | 0:bf96e953cdb8 | 157 | |
echo_piyo | 0:bf96e953cdb8 | 158 | int getCount(){ |
echo_piyo | 0:bf96e953cdb8 | 159 | return count; |
echo_piyo | 0:bf96e953cdb8 | 160 | } |
echo_piyo | 0:bf96e953cdb8 | 161 | |
echo_piyo | 5:6efda58ff71b | 162 | int getYaw_state(){ |
echo_piyo | 5:6efda58ff71b | 163 | return yaw_state; |
echo_piyo | 5:6efda58ff71b | 164 | } |
echo_piyo | 5:6efda58ff71b | 165 | |
echo_piyo | 0:bf96e953cdb8 | 166 | |
echo_piyo | 0:bf96e953cdb8 | 167 | private : |
echo_piyo | 0:bf96e953cdb8 | 168 | Ticker cal_timer; |
echo_piyo | 0:bf96e953cdb8 | 169 | |
echo_piyo | 0:bf96e953cdb8 | 170 | short BNO055_dataBox[12]; |
echo_piyo | 0:bf96e953cdb8 | 171 | short shortdata_yaw; |
echo_piyo | 0:bf96e953cdb8 | 172 | int count; |
echo_piyo | 0:bf96e953cdb8 | 173 | int yaw_state; |
echo_piyo | 0:bf96e953cdb8 | 174 | float data_scAcc, data_scMag, data_scGyro, data_scEUL, data_scTemp; |
echo_piyo | 0:bf96e953cdb8 | 175 | float data_ax, data_ay, data_az, |
echo_piyo | 0:bf96e953cdb8 | 176 | data_mx, data_my, data_mz, |
echo_piyo | 0:bf96e953cdb8 | 177 | data_gx, data_gy, data_gz, |
echo_piyo | 0:bf96e953cdb8 | 178 | data_yaw, data_roll, data_pitch, |
echo_piyo | 0:bf96e953cdb8 | 179 | data_temp, |
echo_piyo | 0:bf96e953cdb8 | 180 | data_firstYaw, data_yaw180, data_yaw360, data_yaw360Rev |
echo_piyo | 0:bf96e953cdb8 | 181 | ,data_yawRad; |
echo_piyo | 0:bf96e953cdb8 | 182 | |
echo_piyo | 0:bf96e953cdb8 | 183 | void getData(){ |
echo_piyo | 0:bf96e953cdb8 | 184 | shortdata_yaw = sensor1.getEulerDataHeading(); |
echo_piyo | 0:bf96e953cdb8 | 185 | data_yaw = (float)shortdata_yaw * data_scEUL; |
echo_piyo | 0:bf96e953cdb8 | 186 | getYaw180(); |
echo_piyo | 0:bf96e953cdb8 | 187 | getYaw360(); |
echo_piyo | 0:bf96e953cdb8 | 188 | getYaw360Rev(); |
echo_piyo | 0:bf96e953cdb8 | 189 | calc_yawRad(); |
echo_piyo | 0:bf96e953cdb8 | 190 | } |
echo_piyo | 0:bf96e953cdb8 | 191 | }; |