6/22 上下往復完成版

Dependencies:   mbed

Fork of BLE_WallbotBLE_Challenge_byYUTAKA3 by Maiko Matsumoto

Files at this revision

API Documentation at this revision

Comitter:
Dyotty
Date:
Fri Jun 22 05:54:44 2018 +0000
Parent:
12:252acb9c1201
Child:
14:dd4adc577e36
Commit message:
6/22 ???

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
--- a/main.cpp	Mon Jun 18 08:43:33 2018 +0000
+++ b/main.cpp	Fri Jun 22 05:54:44 2018 +0000
@@ -370,31 +370,33 @@
     
     // 値を格納する用の配列、変数
     float   acData[3];
-    float   gyData[3];
+//    float   gyData[3];
     float ax = 0;
     float ay = 0;
-    float az = 0;
-    float a = 0;
-    float buf_ax[10];
-    float mean_ax = 0;
-    float sum_ax = 0;
-    float log_ax[100];
+//    float az = 0;
+//    float a = 0;
+//    float buf_ax[10];
+//    float mean_ax = 0;
+//    float dev_ax = 0;
+//    float sum_ax = 0;
+//    float log_ax[100];
     int cnt_loop = 0;
     int cnt_back = 0;
     int cnt_straight = 0;
-    float thre_bump = 4.0;
+//    float thre_bump = 8.0;
+//    float thre_bump_back = 11.0;
     bool enable_ChangeMode = false;
     
-        for(int i = 0; i < 10; i++)
-        {
-            buf_ax[i] = 0;
-        }
+//        for(int i = 0; i < 10; i++)
+//        {
+//            buf_ax[i] = 0;
+//        }
     
         
-        for(int i = 0; i < 100; i++)
-        {
-            log_ax[i] = 0;
-        }
+//        for(int i = 0; i < 100; i++)
+//        {
+//            log_ax[i] = 0;
+//        }
     
     while(sw1 != 0)
     {
@@ -402,20 +404,20 @@
         
         // Get value                   
         //加速度を取得
-        Timer acTimer;
-        acTimer.start();
+//        Timer acTimer;
+//        acTimer.start();
         mpu.getAccelero(acData);    //加速度を取得 acDataに格納
-        acTimer.stop();
+//        acTimer.stop();
     //  at = acTimer.read_ms();
-        acTimer.reset();
+//        acTimer.reset();
      
         //ジャイロを取得
-        Timer gyTimer;
-        gyTimer.start();
-        mpu.getGyro(gyData);    //ジャイロの値(角加速度)を取得 gyDataに格納
-        gyTimer.stop();
+//        Timer gyTimer;
+//        gyTimer.start();
+//        mpu.getGyro(gyData);    //ジャイロの値(角加速度)を取得 gyDataに格納
+//        gyTimer.stop();
     //  gt = gyTimer.read_ms();
-        gyTimer.reset();
+//        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]);  
@@ -423,7 +425,7 @@
 
         ax = acData[0];
         ay = acData[1];
-        az = acData[2];
+//        az = acData[2];
 //        a = sqrt(ax * ax + ay * ay + az * az);
 //        pc.printf("%5.3f, ", ax);
         
@@ -438,49 +440,32 @@
 //            }
         
         // ax Buffer Store
-        for(int i = 9; i > 0; i--)
-        {
-            buf_ax[i] = buf_ax[i - 1];
-            }
-        buf_ax[0] = ax;
-        sum_ax += buf_ax[1];
-        sum_ax -= buf_ax[9];
-        mean_ax = sum_ax / 9;
-        pc.printf("%5.3f, ", ax);
-//        
-//        for(int i = 29; i > 0; i--)
+//        sum_ax -= buf_ax[9];
+//        for(int i = 9; i > 0; i--)
 //        {
-//            log_ax[i] = log_ax[i - 1];
+//            buf_ax[i] = buf_ax[i - 1];
 //            }
-//            log_ax[0] = ax;
+//        buf_ax[0] = ax;
+//        sum_ax += buf_ax[5];
+//        mean_ax = sum_ax / 5;
         
-        if(cnt_loop % 100 == 0)
-        {
-//                        for(int i = 0; i < 100; i++)
-//            {
-//                pc.printf("%5.3f, ", log_ax[i]);
-//                }
-//            pc.printf("loop : %d\n", log_ax);
-//            pc.printf("loop : %d\n", cnt_loop);
-//            pc.printf("loop : %d\n", cnt_loop);
-//            pc.printf("loop : %d\n", cnt_loop);
-//            pc.printf("loop : %d\n", cnt_loop);
-            }
+        // Calculate Deviation
+//        dev_ax = 0;
+//        for(int i = 0; i < 5; i++)
+//        {
+//            dev_ax += (mean_ax - buf_ax[i]) * (mean_ax - buf_ax[i]);
+//            }
+//        dev_ax = sqrt(dev_ax);
+        //
+//        pc.printf("%5.3f, 5.3f \n", ax, ay);
+        
         
 #if 1
         switch(stt_Mode)
         {
             case 1:                 // Up Straight
 //                pc.printf("Mode 1 \n");
-            
-//            for(int i = 0; i < 30; i++)
-//            {
-//                pc.printf("%5.3f, ", log_ax[i]);
-//                }
-//                pc.printf("\n");
-//                pc.printf("\n");
-//                pc.printf("\n");
-            
+                        
                 if(ay > 0.5){
                         left = 0.9;
                         right = 1.0;
@@ -500,10 +485,11 @@
                     // Judge Bump
                     if(enable_ChangeMode)
                     {                        
-                        if(ax - buf_ax[5] < -thre_bump || ax - buf_ax[5] > thre_bump)
+                        if(ay > 4/*ax - mean_ax < -thre_bump || ax - mean_ax > thre_bump*/)
                         {
                             stt_Mode = 2;
                             pc.printf("Mode 1 -> 2 \n");
+                            cnt_back = 0;
                             
                             enable_ChangeMode = false;
                             }
@@ -519,23 +505,24 @@
                 
                 break;
             case 2:             // Up Back
-//                pc.printf("Mode 2 \n");
-                if(cnt_back < 30)
-                {
-                    left = 0;//-1.0;
-                    right = 0;//-1.0;
-                    //cnt_back++;
-                    }
-                    else{
-                            cnt_back = 0;
+                pc.printf("Mode 2 \n");
+//                if(cnt_back < 120)
+//                {
+                    left = -1.0;
+                    right = -1.0;
+//                    cnt_back++;
+                    wait(0.5);
+//                    }
+//                    else{
+//                            cnt_back = 0;
                             stt_Mode = 3;
                             pc.printf("Mode 2 -> 3 \n");
-                        }
+//                        }
                 
                 break;
             case 3:                 // Up Rotate
                 pc.printf("Mode 3 \n");
-                if(!(ax < -9.7/* && ay < 0.5 && ay >= -0.3*/))      // Change Using Gyro??
+                if(!(ax < -9.7 && ay < 0/* && ay >= -0.3*/))      // Change Using Gyro??
                 {
                     left = 1.0;
                     right = 0;                    
@@ -568,7 +555,7 @@
                     // Judge Bump
                     if(enable_ChangeMode)
                     {
-                        if(ax - buf_ax[1] > thre_bump /*&& (ax - buf_ax[2] > thre_bump)*/)
+                        if(ay < -4/*ax - mean_ax < -thre_bump_back || ax - mean_ax > thre_bump_back*/)
                         {
                             stt_Mode = 5;
                             pc.printf("Mode 4 -> 5 \n");
@@ -578,7 +565,7 @@
                     }else
                     {                        
                         cnt_straight++;
-                        if(cnt_straight > 10)
+                        if(cnt_straight > 100)
                         {
                             enable_ChangeMode = true;
                             cnt_straight = 0;
@@ -588,22 +575,23 @@
                 break;
             case 5:           
                 pc.printf("Mode 5 \n");
-                if(cnt_back < 30)
-                {
+//                if(cnt_back < 200)
+//                {
                     left = -1.0;
                     right = -1.0;
-                    cnt_back++;
-                    }
-                    else{
-                            cnt_back = 0;
+                    wait(0.9);
+//                    cnt_back++;
+//                    }
+//                    else{
+//                            cnt_back = 0;
                             stt_Mode = 6;
                             pc.printf(" 5 -> 6 \n");
-                        }
+//                        }
                 
                 break;
             case 6:                 //
                 pc.printf("Mode 6 \n");
-                if(!(ax > 9.7 /*&& ay < 0.5 && ay >= 0*/))      // Change Using Gyro??
+                if(!(ax > 9.7 && ay < 0/* && ay >= 0*/))      // Change Using Gyro??
                 {
                     left = 0;
                     right = 1.0;