ライブラリ化を行った後
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@0:bf96e953cdb8, 2017-06-26 (annotated)
- Committer:
- echo_piyo
- Date:
- Mon Jun 26 09:59:14 2017 +0000
- Revision:
- 0:bf96e953cdb8
- Child:
- 1:2d878962e6ea
????????????????
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 | 0:bf96e953cdb8 | 8 | .begin(); //センサ起動処理 |
echo_piyo | 0:bf96e953cdb8 | 9 | wait(1); //センサの位置取得待ち時間 |
echo_piyo | 0:bf96e953cdb8 | 10 | .firstRead(); //初期値設定 |
echo_piyo | 0:bf96e953cdb8 | 11 | |
echo_piyo | 0:bf96e953cdb8 | 12 | [Function] |
echo_piyo | 0:bf96e953cdb8 | 13 | .begin() //センサ起動処理 |
echo_piyo | 0:bf96e953cdb8 | 14 | .firstRead() //初期値設定(センサ正面を0度設定) |
echo_piyo | 0:bf96e953cdb8 | 15 | .getYaw() //Yawセンサ値(回転角上限なし) 値取得 (返り値 float) |
echo_piyo | 0:bf96e953cdb8 | 16 | .getYaw180() //Yaw(-180~0~180) 値取得 (返り値 float) |
echo_piyo | 0:bf96e953cdb8 | 17 | .getYaw360() //Yaw(0~360)(時計回り) 値取得 (返り値 float) |
echo_piyo | 0:bf96e953cdb8 | 18 | .getYaw360Rev() //Yaw(0~360)(反時計回り) 値取得 (返り値 float) |
echo_piyo | 0:bf96e953cdb8 | 19 | |
echo_piyo | 0:bf96e953cdb8 | 20 | **************************************************/ |
echo_piyo | 0:bf96e953cdb8 | 21 | //p28 p27 or p9 p10 |
echo_piyo | 0:bf96e953cdb8 | 22 | |
echo_piyo | 0:bf96e953cdb8 | 23 | #define BNO005_SDA p9 |
echo_piyo | 0:bf96e953cdb8 | 24 | #define BNO005_SCL p10 |
echo_piyo | 0:bf96e953cdb8 | 25 | #define timer_dt 0.005 |
echo_piyo | 0:bf96e953cdb8 | 26 | #define ifaceI2C_frequency 200000 |
echo_piyo | 0:bf96e953cdb8 | 27 | |
echo_piyo | 0:bf96e953cdb8 | 28 | I2C ifaceI2C(BNO005_SDA,BNO005_SCL);//BNO055用 I2C宣言 |
echo_piyo | 0:bf96e953cdb8 | 29 | BOARDC_BNO055 sensor1(&ifaceI2C); //BNO055用 ライブラリ使用宣言 |
echo_piyo | 0:bf96e953cdb8 | 30 | |
echo_piyo | 0:bf96e953cdb8 | 31 | class Bno055 { |
echo_piyo | 0:bf96e953cdb8 | 32 | public : |
echo_piyo | 0:bf96e953cdb8 | 33 | Bno055(){ |
echo_piyo | 0:bf96e953cdb8 | 34 | shortdata_yaw = 0; |
echo_piyo | 0:bf96e953cdb8 | 35 | count = 0; |
echo_piyo | 0:bf96e953cdb8 | 36 | data_scEUL = 0; |
echo_piyo | 0:bf96e953cdb8 | 37 | data_yaw = 0; |
echo_piyo | 0:bf96e953cdb8 | 38 | data_firstYaw = 0; |
echo_piyo | 0:bf96e953cdb8 | 39 | data_yaw180 = 0; |
echo_piyo | 0:bf96e953cdb8 | 40 | data_yaw360 = 0; |
echo_piyo | 0:bf96e953cdb8 | 41 | data_yaw360Rev = 0; |
echo_piyo | 0:bf96e953cdb8 | 42 | data_yawRad = 0; |
echo_piyo | 0:bf96e953cdb8 | 43 | } |
echo_piyo | 0:bf96e953cdb8 | 44 | |
echo_piyo | 0:bf96e953cdb8 | 45 | void begin(){ |
echo_piyo | 0:bf96e953cdb8 | 46 | cal_timer.attach(this, &Bno055::getData, timer_dt); |
echo_piyo | 0:bf96e953cdb8 | 47 | ifaceI2C.frequency(ifaceI2C_frequency); |
echo_piyo | 0:bf96e953cdb8 | 48 | sensor1.initialize(true); |
echo_piyo | 0:bf96e953cdb8 | 49 | yaw_state = 1; |
echo_piyo | 0:bf96e953cdb8 | 50 | } |
echo_piyo | 0:bf96e953cdb8 | 51 | |
echo_piyo | 0:bf96e953cdb8 | 52 | void firstRead(){ |
echo_piyo | 0:bf96e953cdb8 | 53 | data_scEUL = sensor1.getEulerScale(); |
echo_piyo | 0:bf96e953cdb8 | 54 | shortdata_yaw = sensor1.getEulerDataHeading(); |
echo_piyo | 0:bf96e953cdb8 | 55 | data_firstYaw = (float)shortdata_yaw * data_scEUL; |
echo_piyo | 0:bf96e953cdb8 | 56 | } |
echo_piyo | 0:bf96e953cdb8 | 57 | |
echo_piyo | 0:bf96e953cdb8 | 58 | float getYaw(){ |
echo_piyo | 0:bf96e953cdb8 | 59 | return data_yaw; |
echo_piyo | 0:bf96e953cdb8 | 60 | } |
echo_piyo | 0:bf96e953cdb8 | 61 | |
echo_piyo | 0:bf96e953cdb8 | 62 | float getFirstYaw(){ |
echo_piyo | 0:bf96e953cdb8 | 63 | return data_firstYaw; |
echo_piyo | 0:bf96e953cdb8 | 64 | } |
echo_piyo | 0:bf96e953cdb8 | 65 | |
echo_piyo | 0:bf96e953cdb8 | 66 | float getscEUL(){ |
echo_piyo | 0:bf96e953cdb8 | 67 | return data_scEUL; |
echo_piyo | 0:bf96e953cdb8 | 68 | } |
echo_piyo | 0:bf96e953cdb8 | 69 | |
echo_piyo | 0:bf96e953cdb8 | 70 | float getYaw180(){ |
echo_piyo | 0:bf96e953cdb8 | 71 | //正面0基準変換 |
echo_piyo | 0:bf96e953cdb8 | 72 | if(data_yaw < data_firstYaw){ |
echo_piyo | 0:bf96e953cdb8 | 73 | data_yaw180 = 360.0 + data_yaw - data_firstYaw; |
echo_piyo | 0:bf96e953cdb8 | 74 | }else{ |
echo_piyo | 0:bf96e953cdb8 | 75 | data_yaw180 = data_yaw - data_firstYaw; |
echo_piyo | 0:bf96e953cdb8 | 76 | } |
echo_piyo | 0:bf96e953cdb8 | 77 | //-180~0~180に変換 |
echo_piyo | 0:bf96e953cdb8 | 78 | if(data_yaw180 >= 180){ |
echo_piyo | 0:bf96e953cdb8 | 79 | data_yaw180 = (360 - data_yaw180) * -1; |
echo_piyo | 0:bf96e953cdb8 | 80 | } |
echo_piyo | 0:bf96e953cdb8 | 81 | return data_yaw180; |
echo_piyo | 0:bf96e953cdb8 | 82 | } |
echo_piyo | 0:bf96e953cdb8 | 83 | |
echo_piyo | 0:bf96e953cdb8 | 84 | float getYaw360(){ |
echo_piyo | 0:bf96e953cdb8 | 85 | //正面0基準変換 |
echo_piyo | 0:bf96e953cdb8 | 86 | if(data_yaw < data_firstYaw){ |
echo_piyo | 0:bf96e953cdb8 | 87 | data_yaw360 = 360.0 + data_yaw - data_firstYaw; |
echo_piyo | 0:bf96e953cdb8 | 88 | }else{ |
echo_piyo | 0:bf96e953cdb8 | 89 | data_yaw360 = data_yaw - data_firstYaw; |
echo_piyo | 0:bf96e953cdb8 | 90 | } |
echo_piyo | 0:bf96e953cdb8 | 91 | return data_yaw360; |
echo_piyo | 0:bf96e953cdb8 | 92 | } |
echo_piyo | 0:bf96e953cdb8 | 93 | |
echo_piyo | 0:bf96e953cdb8 | 94 | float getYaw360Rev(){ |
echo_piyo | 0:bf96e953cdb8 | 95 | //正面0基準変換 |
echo_piyo | 0:bf96e953cdb8 | 96 | if(data_yaw < data_firstYaw){ |
echo_piyo | 0:bf96e953cdb8 | 97 | data_yaw360Rev = 360.0 + data_yaw - data_firstYaw; |
echo_piyo | 0:bf96e953cdb8 | 98 | }else{ |
echo_piyo | 0:bf96e953cdb8 | 99 | data_yaw360Rev = data_yaw - data_firstYaw; |
echo_piyo | 0:bf96e953cdb8 | 100 | } |
echo_piyo | 0:bf96e953cdb8 | 101 | //反時計周りに増加 |
echo_piyo | 0:bf96e953cdb8 | 102 | data_yaw360Rev = 360 - data_yaw360Rev; |
echo_piyo | 0:bf96e953cdb8 | 103 | return data_yaw360Rev; |
echo_piyo | 0:bf96e953cdb8 | 104 | } |
echo_piyo | 0:bf96e953cdb8 | 105 | |
echo_piyo | 0:bf96e953cdb8 | 106 | float getYawRad(){ |
echo_piyo | 0:bf96e953cdb8 | 107 | return data_yawRad; |
echo_piyo | 0:bf96e953cdb8 | 108 | } |
echo_piyo | 0:bf96e953cdb8 | 109 | |
echo_piyo | 0:bf96e953cdb8 | 110 | int getYawState(){ |
echo_piyo | 0:bf96e953cdb8 | 111 | return yaw_state; |
echo_piyo | 0:bf96e953cdb8 | 112 | } |
echo_piyo | 0:bf96e953cdb8 | 113 | |
echo_piyo | 0:bf96e953cdb8 | 114 | void calc_yawRad(){ |
echo_piyo | 0:bf96e953cdb8 | 115 | switch(yaw_state){ |
echo_piyo | 0:bf96e953cdb8 | 116 | |
echo_piyo | 0:bf96e953cdb8 | 117 | case 1: |
echo_piyo | 0:bf96e953cdb8 | 118 | if(data_yaw180 >= 90){ |
echo_piyo | 0:bf96e953cdb8 | 119 | yaw_state = 2; |
echo_piyo | 0:bf96e953cdb8 | 120 | }else if(data_yaw180 <= -90){ |
echo_piyo | 0:bf96e953cdb8 | 121 | yaw_state = 3; |
echo_piyo | 0:bf96e953cdb8 | 122 | } |
echo_piyo | 0:bf96e953cdb8 | 123 | data_yawRad = 360.0 * count + data_yaw180; |
echo_piyo | 0:bf96e953cdb8 | 124 | break; |
echo_piyo | 0:bf96e953cdb8 | 125 | |
echo_piyo | 0:bf96e953cdb8 | 126 | case 2: |
echo_piyo | 0:bf96e953cdb8 | 127 | if(data_yaw360 > 270){ |
echo_piyo | 0:bf96e953cdb8 | 128 | yaw_state = 1; |
echo_piyo | 0:bf96e953cdb8 | 129 | count++; |
echo_piyo | 0:bf96e953cdb8 | 130 | }else if(data_yaw360 < 90){ |
echo_piyo | 0:bf96e953cdb8 | 131 | yaw_state = 1; |
echo_piyo | 0:bf96e953cdb8 | 132 | } |
echo_piyo | 0:bf96e953cdb8 | 133 | data_yawRad = 360.0 * count + data_yaw360; |
echo_piyo | 0:bf96e953cdb8 | 134 | break; |
echo_piyo | 0:bf96e953cdb8 | 135 | |
echo_piyo | 0:bf96e953cdb8 | 136 | case 3: |
echo_piyo | 0:bf96e953cdb8 | 137 | if(data_yaw360 < 90){ |
echo_piyo | 0:bf96e953cdb8 | 138 | yaw_state = 1; |
echo_piyo | 0:bf96e953cdb8 | 139 | count--; |
echo_piyo | 0:bf96e953cdb8 | 140 | }else if(data_yaw360 > 270){ |
echo_piyo | 0:bf96e953cdb8 | 141 | yaw_state = 1; |
echo_piyo | 0:bf96e953cdb8 | 142 | } |
echo_piyo | 0:bf96e953cdb8 | 143 | data_yawRad = 360.0 * count - data_yaw360Rev; |
echo_piyo | 0:bf96e953cdb8 | 144 | break; |
echo_piyo | 0:bf96e953cdb8 | 145 | } |
echo_piyo | 0:bf96e953cdb8 | 146 | } |
echo_piyo | 0:bf96e953cdb8 | 147 | |
echo_piyo | 0:bf96e953cdb8 | 148 | int getCount(){ |
echo_piyo | 0:bf96e953cdb8 | 149 | return count; |
echo_piyo | 0:bf96e953cdb8 | 150 | } |
echo_piyo | 0:bf96e953cdb8 | 151 | |
echo_piyo | 0:bf96e953cdb8 | 152 | |
echo_piyo | 0:bf96e953cdb8 | 153 | private : |
echo_piyo | 0:bf96e953cdb8 | 154 | Ticker cal_timer; |
echo_piyo | 0:bf96e953cdb8 | 155 | |
echo_piyo | 0:bf96e953cdb8 | 156 | short BNO055_dataBox[12]; |
echo_piyo | 0:bf96e953cdb8 | 157 | short shortdata_yaw; |
echo_piyo | 0:bf96e953cdb8 | 158 | int count; |
echo_piyo | 0:bf96e953cdb8 | 159 | int yaw_state; |
echo_piyo | 0:bf96e953cdb8 | 160 | float data_scAcc, data_scMag, data_scGyro, data_scEUL, data_scTemp; |
echo_piyo | 0:bf96e953cdb8 | 161 | float data_ax, data_ay, data_az, |
echo_piyo | 0:bf96e953cdb8 | 162 | data_mx, data_my, data_mz, |
echo_piyo | 0:bf96e953cdb8 | 163 | data_gx, data_gy, data_gz, |
echo_piyo | 0:bf96e953cdb8 | 164 | data_yaw, data_roll, data_pitch, |
echo_piyo | 0:bf96e953cdb8 | 165 | data_temp, |
echo_piyo | 0:bf96e953cdb8 | 166 | data_firstYaw, data_yaw180, data_yaw360, data_yaw360Rev |
echo_piyo | 0:bf96e953cdb8 | 167 | ,data_yawRad; |
echo_piyo | 0:bf96e953cdb8 | 168 | |
echo_piyo | 0:bf96e953cdb8 | 169 | void getData(){ |
echo_piyo | 0:bf96e953cdb8 | 170 | shortdata_yaw = sensor1.getEulerDataHeading(); |
echo_piyo | 0:bf96e953cdb8 | 171 | data_yaw = (float)shortdata_yaw * data_scEUL; |
echo_piyo | 0:bf96e953cdb8 | 172 | getYaw180(); |
echo_piyo | 0:bf96e953cdb8 | 173 | getYaw360(); |
echo_piyo | 0:bf96e953cdb8 | 174 | getYaw360Rev(); |
echo_piyo | 0:bf96e953cdb8 | 175 | calc_yawRad(); |
echo_piyo | 0:bf96e953cdb8 | 176 | } |
echo_piyo | 0:bf96e953cdb8 | 177 | }; |