Diff: bno055_use.h

Revision:

7:52491500cd6d

Parent:

6:23e029bd9c9b

Child:

8:a698dcb837ee

--- a/bno055_use.h	Thu Jul 04 10:24:26 2019 +0000
+++ b/bno055_use.h	Mon Jul 19 08:20:47 2021 +0000
@@ -1,7 +1,7 @@
 /*************************************************
 [使い方]
-1.  #include "bno005_lib.h"     //BNO055 本家ライブラリ
-    #include "bno005_use.h"     //BNO055 データ取得用Class
+1.  #include "bno055_lib.h"     //BNO055 本家ライブラリ
+    #include "bno055_use.h"     //BNO055 データ取得用Class
     を読み込ませる
 2.  bno055_use.hの#defineでI2Cピン指定をする
 3.  main()内での初期設定を行う
@@ -13,12 +13,13 @@
 [Function]
     .begin()        //センサ起動処理
     .firstRead()    //初期値設定(センサ正面を0度設定)
-    .getYaw()       //Yawセンサ値(回転角上限なし)  値取得   (返り値 float)
+    .getYaw()       //Yawセンサ値(回転角上限なし)   値取得   (返り値 float)
     .getYawRad()    //回転行列で使う              値取得   (返り値 float)
     .getYaw180()    //Yaw(-180~0~180)           値取得   (返り値 float)
     .getYaw360()    //Yaw(0~360)(時計回り)       値取得   (返り値 float)
     .getYaw360Rev() //Yaw(0~360)(反時計回り)     値取得   (返り値 float)
     .getYawRad360() //yaw(-360~0~360)          値取得    (返り値 float)
+    .getRoll()      //roll(-90~0~90)           値取得    (返り値 float)
     
 **************************************************/
 //p28 p27 or p9 p10
@@ -61,7 +62,10 @@
         void firstRead(){
             data_scEUL = sensor1.getEulerScale();
             shortdata_yaw = sensor1.getEulerDataHeading();
+            //data_firstYaw = (float)shortdata_yaw * data_scEUL;
             data_firstYaw = (float)shortdata_yaw * data_scEUL;
+            data_firstRoll = (float)shortdata_roll * data_scEUL;
+            data_firstPitch = (float)shortdata_pitch * data_scEUL;
         }
         
         void yaw_origin() {
@@ -116,9 +120,8 @@
             return data_yaw360Rev;
         }
         
-        /***************************************自作BNO055関数**********************************/
         /************************
-            ロボットの回転方向から+360 ~ 0 ~ -360の値を推定して返す
+            ロボットの回転方向から+360 ~ 0 ~ -360の値を推定して返す (Yaw軸)
         *************************/
         float getYawRad360() {
             float calculate_yawRad;
@@ -130,7 +133,6 @@
             } else {
                 calculate_yawRad = data_yawRad;
             }          
-            
             return -calculate_yawRad;
         }
             
@@ -177,16 +179,47 @@
             }
         }
         
+        void calYawTwoWay360(){
+            static float yaw_sign;
+            static float old_yaw;
+            float now_yaw = data_yaw - data_firstYaw;
+            float yaw_diffelence = now_yaw - old_yaw;
+         
+            if(yaw_diffelence >= 100){
+                yaw_sign = 1; // negative
+            } else if(yaw_diffelence <= -100) {
+                yaw_sign = 0; //positive
+            }
+         
+            if(yaw_sign){
+                data_twoway360 = 360.0 - now_yaw;
+            } else {
+                data_twoway360 = -now_yaw;
+            }
+            old_yaw = now_yaw;
+        }
+        
         int getCount(){
             return count;
         }
         
+        float getYawTwoWay360() {
+            return data_twoway360;
+        }
+        
+        float getRoll() {
+            return data_roll - data_firstRoll;
+        }
+         
+        float getPitch() {
+            return data_pitch - data_firstPitch;
+        }   
         
 private :
     Ticker cal_timer;
     
     short BNO055_dataBox[12];
-    short shortdata_yaw;
+    short shortdata_yaw,shortdata_roll,shortdata_pitch;
     int   count;
     int   yaw_state;
     float data_scAcc, data_scMag, data_scGyro, data_scEUL, data_scTemp;
@@ -197,14 +230,20 @@
           data_temp,
           data_firstYaw, data_yaw180, data_yaw360, data_yaw360Rev
           ,data_yawRad;
+    float data_firstRoll,data_firstPitch;
+    float data_twoway360;
         
     void getData(){
-        shortdata_yaw = sensor1.getEulerDataHeading();
+        //shortdata_yaw = sensor1.getEulerDataHeading();
+        sensor1.getEulerDataAll(shortdata_yaw,shortdata_roll,shortdata_pitch);
         data_yaw = (float)shortdata_yaw * data_scEUL;
+        data_roll = (float)shortdata_roll * data_scEUL;
+        data_pitch = (float)shortdata_pitch * data_scEUL;
         getYaw180();
         getYaw360();
         getYaw360Rev();
         calc_yawRad();
+        calYawTwoWay360();
     }
 };