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@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?

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 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 };