Yutaka Yoshida
/
BLE_WallbotBLE_Challenge_byYUTAKA3
6/22 上下往復完成版
Fork of BLE_WallbotBLE_Challenge_byYUTAKA3 by
Revision 13:ce45cdb24996, committed 2018-06-22
- 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;