kusano kiyoshige / Mbed 2 deprecated 17robo_fuzi

ライブラリ化を行った後

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 kusano kiyoshige

bno055_use.h@2:d5b8f8e62923, 2017-06-29 (annotated)

Committer:
echo_piyo
Date:
Thu Jun 29 09:47:49 2017 +0000
Revision:
2:d5b8f8e62923
Parent:
1:2d878962e6ea
Child:
5:6efda58ff71b
bno055????? 10msec; output_timer 5msec

Who changed what in which revision?

UserRevisionLine numberNew 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 0:bf96e953cdb8 162
echo_piyo 0:bf96e953cdb8 163 private :
echo_piyo 0:bf96e953cdb8 164 Ticker cal_timer;
echo_piyo 0:bf96e953cdb8 165
echo_piyo 0:bf96e953cdb8 166 short BNO055_dataBox[12];
echo_piyo 0:bf96e953cdb8 167 short shortdata_yaw;
echo_piyo 0:bf96e953cdb8 168 int count;
echo_piyo 0:bf96e953cdb8 169 int yaw_state;
echo_piyo 0:bf96e953cdb8 170 float data_scAcc, data_scMag, data_scGyro, data_scEUL, data_scTemp;
echo_piyo 0:bf96e953cdb8 171 float data_ax, data_ay, data_az,
echo_piyo 0:bf96e953cdb8 172 data_mx, data_my, data_mz,
echo_piyo 0:bf96e953cdb8 173 data_gx, data_gy, data_gz,
echo_piyo 0:bf96e953cdb8 174 data_yaw, data_roll, data_pitch,
echo_piyo 0:bf96e953cdb8 175 data_temp,
echo_piyo 0:bf96e953cdb8 176 data_firstYaw, data_yaw180, data_yaw360, data_yaw360Rev
echo_piyo 0:bf96e953cdb8 177 ,data_yawRad;
echo_piyo 0:bf96e953cdb8 178
echo_piyo 0:bf96e953cdb8 179 void getData(){
echo_piyo 0:bf96e953cdb8 180 shortdata_yaw = sensor1.getEulerDataHeading();
echo_piyo 0:bf96e953cdb8 181 data_yaw = (float)shortdata_yaw * data_scEUL;
echo_piyo 0:bf96e953cdb8 182 getYaw180();
echo_piyo 0:bf96e953cdb8 183 getYaw360();
echo_piyo 0:bf96e953cdb8 184 getYaw360Rev();
echo_piyo 0:bf96e953cdb8 185 calc_yawRad();
echo_piyo 0:bf96e953cdb8 186 }
echo_piyo 0:bf96e953cdb8 187 };