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