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@1:2d878962e6ea, 2017-06-29 (annotated)

Committer:
echo_piyo
Date:
Thu Jun 29 08:52:40 2017 +0000
Revision:
1:2d878962e6ea
Parent:
0:bf96e953cdb8
Child:
2:d5b8f8e62923
bno055_use.h???????????????????????????????; ?????10msec????; 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 0:bf96e953cdb8 51 }
echo_piyo 0:bf96e953cdb8 52
echo_piyo 0:bf96e953cdb8 53 void firstRead(){
echo_piyo 0:bf96e953cdb8 54 data_scEUL = sensor1.getEulerScale();
echo_piyo 0:bf96e953cdb8 55 shortdata_yaw = sensor1.getEulerDataHeading();
echo_piyo 0:bf96e953cdb8 56 data_firstYaw = (float)shortdata_yaw * data_scEUL;
echo_piyo 0:bf96e953cdb8 57 }
echo_piyo 0:bf96e953cdb8 58
echo_piyo 0:bf96e953cdb8 59 float getYaw(){
echo_piyo 0:bf96e953cdb8 60 return data_yaw;
echo_piyo 0:bf96e953cdb8 61 }
echo_piyo 0:bf96e953cdb8 62
echo_piyo 0:bf96e953cdb8 63 float getFirstYaw(){
echo_piyo 0:bf96e953cdb8 64 return data_firstYaw;
echo_piyo 0:bf96e953cdb8 65 }
echo_piyo 0:bf96e953cdb8 66
echo_piyo 0:bf96e953cdb8 67 float getscEUL(){
echo_piyo 0:bf96e953cdb8 68 return data_scEUL;
echo_piyo 0:bf96e953cdb8 69 }
echo_piyo 0:bf96e953cdb8 70
echo_piyo 0:bf96e953cdb8 71 float getYaw180(){
echo_piyo 0:bf96e953cdb8 72 //正面0基準変換
echo_piyo 0:bf96e953cdb8 73 if(data_yaw < data_firstYaw){
echo_piyo 0:bf96e953cdb8 74 data_yaw180 = 360.0 + data_yaw - data_firstYaw;
echo_piyo 0:bf96e953cdb8 75 }else{
echo_piyo 0:bf96e953cdb8 76 data_yaw180 = data_yaw - data_firstYaw;
echo_piyo 0:bf96e953cdb8 77 }
echo_piyo 0:bf96e953cdb8 78 //-180~0~180に変換
echo_piyo 0:bf96e953cdb8 79 if(data_yaw180 >= 180){
echo_piyo 0:bf96e953cdb8 80 data_yaw180 = (360 - data_yaw180) * -1;
echo_piyo 0:bf96e953cdb8 81 }
echo_piyo 0:bf96e953cdb8 82 return data_yaw180;
echo_piyo 0:bf96e953cdb8 83 }
echo_piyo 0:bf96e953cdb8 84
echo_piyo 0:bf96e953cdb8 85 float getYaw360(){
echo_piyo 0:bf96e953cdb8 86 //正面0基準変換
echo_piyo 0:bf96e953cdb8 87 if(data_yaw < data_firstYaw){
echo_piyo 0:bf96e953cdb8 88 data_yaw360 = 360.0 + data_yaw - data_firstYaw;
echo_piyo 0:bf96e953cdb8 89 }else{
echo_piyo 0:bf96e953cdb8 90 data_yaw360 = data_yaw - data_firstYaw;
echo_piyo 0:bf96e953cdb8 91 }
echo_piyo 0:bf96e953cdb8 92 return data_yaw360;
echo_piyo 0:bf96e953cdb8 93 }
echo_piyo 0:bf96e953cdb8 94
echo_piyo 0:bf96e953cdb8 95 float getYaw360Rev(){
echo_piyo 0:bf96e953cdb8 96 //正面0基準変換
echo_piyo 0:bf96e953cdb8 97 if(data_yaw < data_firstYaw){
echo_piyo 0:bf96e953cdb8 98 data_yaw360Rev = 360.0 + data_yaw - data_firstYaw;
echo_piyo 0:bf96e953cdb8 99 }else{
echo_piyo 0:bf96e953cdb8 100 data_yaw360Rev = data_yaw - data_firstYaw;
echo_piyo 0:bf96e953cdb8 101 }
echo_piyo 0:bf96e953cdb8 102 //反時計周りに増加
echo_piyo 0:bf96e953cdb8 103 data_yaw360Rev = 360 - data_yaw360Rev;
echo_piyo 0:bf96e953cdb8 104 return data_yaw360Rev;
echo_piyo 0:bf96e953cdb8 105 }
echo_piyo 0:bf96e953cdb8 106
echo_piyo 0:bf96e953cdb8 107 float getYawRad(){
echo_piyo 0:bf96e953cdb8 108 return data_yawRad;
echo_piyo 0:bf96e953cdb8 109 }
echo_piyo 0:bf96e953cdb8 110
echo_piyo 0:bf96e953cdb8 111 int getYawState(){
echo_piyo 0:bf96e953cdb8 112 return yaw_state;
echo_piyo 0:bf96e953cdb8 113 }
echo_piyo 0:bf96e953cdb8 114
echo_piyo 0:bf96e953cdb8 115 void calc_yawRad(){
echo_piyo 0:bf96e953cdb8 116 switch(yaw_state){
echo_piyo 0:bf96e953cdb8 117
echo_piyo 0:bf96e953cdb8 118 case 1:
echo_piyo 0:bf96e953cdb8 119 if(data_yaw180 >= 90){
echo_piyo 0:bf96e953cdb8 120 yaw_state = 2;
echo_piyo 0:bf96e953cdb8 121 }else if(data_yaw180 <= -90){
echo_piyo 0:bf96e953cdb8 122 yaw_state = 3;
echo_piyo 0:bf96e953cdb8 123 }
echo_piyo 0:bf96e953cdb8 124 data_yawRad = 360.0 * count + data_yaw180;
echo_piyo 0:bf96e953cdb8 125 break;
echo_piyo 0:bf96e953cdb8 126
echo_piyo 0:bf96e953cdb8 127 case 2:
echo_piyo 0:bf96e953cdb8 128 if(data_yaw360 > 270){
echo_piyo 0:bf96e953cdb8 129 yaw_state = 1;
echo_piyo 0:bf96e953cdb8 130 count++;
echo_piyo 0:bf96e953cdb8 131 }else if(data_yaw360 < 90){
echo_piyo 0:bf96e953cdb8 132 yaw_state = 1;
echo_piyo 0:bf96e953cdb8 133 }
echo_piyo 0:bf96e953cdb8 134 data_yawRad = 360.0 * count + data_yaw360;
echo_piyo 0:bf96e953cdb8 135 break;
echo_piyo 0:bf96e953cdb8 136
echo_piyo 0:bf96e953cdb8 137 case 3:
echo_piyo 0:bf96e953cdb8 138 if(data_yaw360 < 90){
echo_piyo 0:bf96e953cdb8 139 yaw_state = 1;
echo_piyo 0:bf96e953cdb8 140 count--;
echo_piyo 0:bf96e953cdb8 141 }else if(data_yaw360 > 270){
echo_piyo 0:bf96e953cdb8 142 yaw_state = 1;
echo_piyo 0:bf96e953cdb8 143 }
echo_piyo 0:bf96e953cdb8 144 data_yawRad = 360.0 * count - data_yaw360Rev;
echo_piyo 0:bf96e953cdb8 145 break;
echo_piyo 0:bf96e953cdb8 146 }
echo_piyo 0:bf96e953cdb8 147 }
echo_piyo 0:bf96e953cdb8 148
echo_piyo 0:bf96e953cdb8 149 int getCount(){
echo_piyo 0:bf96e953cdb8 150 return count;
echo_piyo 0:bf96e953cdb8 151 }
echo_piyo 0:bf96e953cdb8 152
echo_piyo 0:bf96e953cdb8 153
echo_piyo 0:bf96e953cdb8 154 private :
echo_piyo 0:bf96e953cdb8 155 Ticker cal_timer;
echo_piyo 0:bf96e953cdb8 156
echo_piyo 0:bf96e953cdb8 157 short BNO055_dataBox[12];
echo_piyo 0:bf96e953cdb8 158 short shortdata_yaw;
echo_piyo 0:bf96e953cdb8 159 int count;
echo_piyo 0:bf96e953cdb8 160 int yaw_state;
echo_piyo 0:bf96e953cdb8 161 float data_scAcc, data_scMag, data_scGyro, data_scEUL, data_scTemp;
echo_piyo 0:bf96e953cdb8 162 float data_ax, data_ay, data_az,
echo_piyo 0:bf96e953cdb8 163 data_mx, data_my, data_mz,
echo_piyo 0:bf96e953cdb8 164 data_gx, data_gy, data_gz,
echo_piyo 0:bf96e953cdb8 165 data_yaw, data_roll, data_pitch,
echo_piyo 0:bf96e953cdb8 166 data_temp,
echo_piyo 0:bf96e953cdb8 167 data_firstYaw, data_yaw180, data_yaw360, data_yaw360Rev
echo_piyo 0:bf96e953cdb8 168 ,data_yawRad;
echo_piyo 0:bf96e953cdb8 169
echo_piyo 0:bf96e953cdb8 170 void getData(){
echo_piyo 0:bf96e953cdb8 171 shortdata_yaw = sensor1.getEulerDataHeading();
echo_piyo 0:bf96e953cdb8 172 data_yaw = (float)shortdata_yaw * data_scEUL;
echo_piyo 0:bf96e953cdb8 173 getYaw180();
echo_piyo 0:bf96e953cdb8 174 getYaw360();
echo_piyo 0:bf96e953cdb8 175 getYaw360Rev();
echo_piyo 0:bf96e953cdb8 176 calc_yawRad();
echo_piyo 0:bf96e953cdb8 177 }
echo_piyo 0:bf96e953cdb8 178 };