Diff: main.cpp

Revision:

6:213041054f97

Parent:

5:29f7f535d4b2

Child:

7:897b9ded86e2

--- a/main.cpp	Mon Jun 11 07:37:33 2018 +0000
+++ b/main.cpp	Mon Jun 11 11:32:07 2018 +0000
@@ -4,7 +4,7 @@
 #include "TB6612.h"
 #include "MPU6050.h"
 
-#define DBG 0
+#define DBG 1
 
 BLEDevice  ble;
 
@@ -357,45 +357,61 @@
 void wb_control(void)
 {
     ModeLed = 0;
-    wait(1);
+    wait(1);    
     
+    // Wallbot State Initialize
+    // Up Straight  : 1
+    // Up Back      : 2
+    // Up Rotate    : 3
+    // Down Straight  : 4
+    // Down Back      : 5
+    // Down Rotate    : 6
+    stt_Mode = 1;
+    
+    // 値を格納する用の配列、変数
+    float   acData[3];
+    float   gyData[3];
+    float ax = 0;
+    float ay = 0;
     int cnt_loop = 0;
     float pre_ax = 0;
     float thre_bump = 1.0;
     
     while(sw1 != 0)
     {
-        Wait(0.1);
+//        Wait(0.1);
         
-        // Get value
+        // Get value                   
+        //加速度を取得
+        Timer acTimer;
+        acTimer.start();
+        mpu.getAccelero(acData);    //加速度を取得 acDataに格納
+        acTimer.stop();
+    //  at = acTimer.read_ms();
+        acTimer.reset();
+     
+        //ジャイロを取得
+        Timer gyTimer;
+        gyTimer.start();
+        mpu.getGyro(gyData);    //ジャイロの値（角加速度）を取得 gyDataに格納
+        gyTimer.stop();
+    //  gt = gyTimer.read_ms();
+        gyTimer.reset();
+     
+        //floatの値を合計5桁、小数点以下3桁の形でPCへ送信
+        pc.printf("%5.3f:%5.3f:%5.3f:%5.3f:%5.3f:%5.3f \n", acData[0], acData[1], acData[2],gyData[0],gyData[1],gyData[2]);  
+
+        ax = acData[0];
+        ay = acData[1];
         
         // Store pre frame value
-        pre_ax = ax;
-        
+        pre_ax = acData[0];       
         
         
-//#if 0
-//        int line = get_line(0) ? 0 : 1;
-//            line |= get_line(1) ? 0 : 2;
-//            line |= get_line(2) ? 0 : 4;
-//            line |= get_line(3) ? 0 : 8;
-//#else
-//        int line = get_line(0) ? 1 : 0;
-//            line |= get_line(1) ? 2 : 0;
-//            line |= get_line(2) ? 4 : 0;
-//            line |= get_line(3) ? 8 : 0;
-//#endif
-
-//#if DBG
-//        pc.printf("line=%02x %04x %04x %04x %04x\n\r",line,base_fsen[0],base_fsen[1],base_fsen[2],base_fsen[3]);
-//#endif
-
-//改造中のソース
-
 #if 1
         switch(stt_Mode)
         {
-            case 1:                 // Up forward
+            case 1:                 // Up Straight
                 pc.printf("Mode 1");
             
                 if(ay < -0.2){
@@ -413,14 +429,14 @@
                         }
                         
                     // Judge Bump
-                    if(ax - pre_ax < -thre_bump)
+                    if( - pre_ax < -thre_bump)
                     {
                         stt_Mode = 2;
                         pc.printf(" 1 -> 2 ");
                         }
                 
                 break;
-            case 2:           
+            case 2:             // Up Back
                 pc.printf("Mode 2");
                 if(cnt_loop < 10)
                 {
@@ -435,7 +451,7 @@
                         }
                 
                 break;
-            case 3:                 //
+            case 3:                 // Up Rotate
                 pc.printf("Mode 3");
                 if(ax < 9.8 && ay > 0.1 && ay < -0.1)
                 {
@@ -450,7 +466,7 @@
                 
                 break;
             
-            case 4:                 // Down forward
+            case 4:                 // Down Straight
                 pc.printf("Mode 4");
                 if(ay < -0.2){
                         left = 1.0;
@@ -575,10 +591,18 @@
         }
     }
 
-    // MPU6050 Initialize
+//    // MPU6050 Initialize
+//    mpu.initialize();
+//    mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_8G);
+//    mpu.setGyroRange(MPU6050_GYRO_RANGE_1000);
+
+    ///180601 MPU6050センサの初期化処理
     mpu.initialize();
-    mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_8G);
-    mpu.setGyroRange(MPU6050_GYRO_RANGE_1000);
+//  mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_8G);    //加速度センサ 計測レンジ設定（今回は2Gか4Gがよさそう）
+    mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G); 
+//   mpu.setGyroRange(MPU6050_GYRO_RANGE_1000);          //ジャイロセンサ 計測レンジ設定（ここも250か500がよさそう（そんなに早く回転しないので））
+    mpu.setGyroRange(MPU6050_GYRO_RANGE_250);
+    ///180601 
 
     ble.init(); 
     ble.onConnection(onConnected);
@@ -597,16 +621,7 @@
     ble.startAdvertising();
 
     ble.addService(RCBControllerService);
-    
 
-    // Wallbot State Initialize
-    // Up Straight  : 1
-    // Up Back      : 2
-    // Up Rotate    : 3
-    // Down Straight  : 4
-    // Down Back      : 5
-    // Down Rotate    : 6
-    stt_Mode = 1;
 
     while (true) {
         if(sw1 == 0)