mbed_robotcar / Mbed OS avoidance

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

main.cpp@0:5d6b3a226854, 2020-07-22 (annotated)

Committer:
tomotsugu
Date:
Wed Jul 22 02:08:10 2020 +0000
Revision:
0:5d6b3a226854
Child:
1:9787fdc9b191
avoidance

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tomotsugu 0:5d6b3a226854 1 #include "mbed.h"
tomotsugu 0:5d6b3a226854 2 #include "rtos.h"
tomotsugu 0:5d6b3a226854 3
tomotsugu 0:5d6b3a226854 4 DigitalOut trig(p6); // 超音波センサのtrigピンをp6に接続
tomotsugu 0:5d6b3a226854 5 DigitalIn echo(p7); // 超音波センサのechoピンをp7に接続
tomotsugu 0:5d6b3a226854 6 PwmOut servo(p25); // サーボコントロールピン(p25)
tomotsugu 0:5d6b3a226854 7
tomotsugu 0:5d6b3a226854 8 Timer timer; // 距離計測用タイマー
tomotsugu 0:5d6b3a226854 9
tomotsugu 0:5d6b3a226854 10 /* 障害物検知用の変数設定 */
tomotsugu 0:5d6b3a226854 11 int SC; // 正面
tomotsugu 0:5d6b3a226854 12 int SL; // 左
tomotsugu 0:5d6b3a226854 13 int SR; // 右
tomotsugu 0:5d6b3a226854 14 int SLD; // 左前
tomotsugu 0:5d6b3a226854 15 int SRD; // 右前
tomotsugu 0:5d6b3a226854 16 int flag = 0; // 障害物有無のフラグ
tomotsugu 0:5d6b3a226854 17 const int limit = 20;; // 障害物の距離のリミット(単位：cm)
tomotsugu 0:5d6b3a226854 18 const int sublimit = 15; // 急な障害物の距離のリミット(単位：cm)
tomotsugu 0:5d6b3a226854 19 int DT; // 距離
tomotsugu 0:5d6b3a226854 20 int houkou; // 進行方向(1:前進 2:左折 3:右折)
tomotsugu 0:5d6b3a226854 21 int max; // 最も遠い距離
tomotsugu 0:5d6b3a226854 22 int i; // ループ変数
tomotsugu 0:5d6b3a226854 23
tomotsugu 0:5d6b3a226854 24 /* 超音波センサ関数 */
tomotsugu 0:5d6b3a226854 25 int watch(){
tomotsugu 0:5d6b3a226854 26 trig = 0;
tomotsugu 0:5d6b3a226854 27 thread_sleep_for(0.5); // 5ms待つ
tomotsugu 0:5d6b3a226854 28 trig = 1;
tomotsugu 0:5d6b3a226854 29 thread_sleep_for(1.5); // 15ms待つ
tomotsugu 0:5d6b3a226854 30 trig = 0;
tomotsugu 0:5d6b3a226854 31 while(echo.read() == 0){
tomotsugu 0:5d6b3a226854 32 }
tomotsugu 0:5d6b3a226854 33 timer.start(); // 距離計測タイマースタート
tomotsugu 0:5d6b3a226854 34 while(echo.read() == 1){
tomotsugu 0:5d6b3a226854 35 }
tomotsugu 0:5d6b3a226854 36 timer.stop(); // 距離計測タイマーストップ
tomotsugu 0:5d6b3a226854 37 DT = timer.read_us()*0.01657; // 距離計算
tomotsugu 0:5d6b3a226854 38 if(DT > 400){ // 検知範囲外なら400cmに設定
tomotsugu 0:5d6b3a226854 39 DT = 400;
tomotsugu 0:5d6b3a226854 40 }
tomotsugu 0:5d6b3a226854 41 timer.reset(); // 距離計測タイマーリセット
tomotsugu 0:5d6b3a226854 42 thread_sleep_for(3); // 30ms待つ
tomotsugu 0:5d6b3a226854 43 return DT;
tomotsugu 0:5d6b3a226854 44 }
tomotsugu 0:5d6b3a226854 45
tomotsugu 0:5d6b3a226854 46 /* サーボ関数 */
tomotsugu 0:5d6b3a226854 47 void watchsurrounding(void const *n){
tomotsugu 0:5d6b3a226854 48 while(1){
tomotsugu 0:5d6b3a226854 49 Thread::signal_wait(0x1); // シグナル待ち
tomotsugu 0:5d6b3a226854 50 SC = watch();
tomotsugu 0:5d6b3a226854 51 if(SC < limit){ // 前方20cm以内に障害物がある場合
tomotsugu 0:5d6b3a226854 52 run = STOP; // 停止
tomotsugu 0:5d6b3a226854 53 }
tomotsugu 0:5d6b3a226854 54 servo.pulsewidth_us(1925); // サーボを左に40度回転
tomotsugu 0:5d6b3a226854 55 thread_sleep_for(25); // 250ms待つ
tomotsugu 0:5d6b3a226854 56 SLD = watch();
tomotsugu 0:5d6b3a226854 57 if(SLD < limit){ // 左斜め20cm以内に障害物がある場合
tomotsugu 0:5d6b3a226854 58 run = STOP; // 停止
tomotsugu 0:5d6b3a226854 59 }
tomotsugu 0:5d6b3a226854 60 servo.pulsewidth_us(2400); // サーボを左に90度回転
tomotsugu 0:5d6b3a226854 61 thread_sleep_for(25); // 250ms待つ
tomotsugu 0:5d6b3a226854 62 SL = watch();
tomotsugu 0:5d6b3a226854 63 if(SL < limit){ // 左20cm以内に障害物がある場合
tomotsugu 0:5d6b3a226854 64 run = STOP; // 停止
tomotsugu 0:5d6b3a226854 65 }
tomotsugu 0:5d6b3a226854 66 servo.pulsewidth_us(1450);
tomotsugu 0:5d6b3a226854 67 thread_sleep_for(10);
tomotsugu 0:5d6b3a226854 68 servo.pulsewidth_us(925); // サーボを右に40度回転
tomotsugu 0:5d6b3a226854 69 thread_sleep_for(25); // 250ms待つ
tomotsugu 0:5d6b3a226854 70 SRD = watch();
tomotsugu 0:5d6b3a226854 71 if(SRD < limit){ // 右斜め20cm以内に障害物がある場合
tomotsugu 0:5d6b3a226854 72 run = STOP; // 停止
tomotsugu 0:5d6b3a226854 73 }
tomotsugu 0:5d6b3a226854 74 servo.pulsewidth_us(500); // サーボを右に90度回転
tomotsugu 0:5d6b3a226854 75 thread_sleep_for(25); // 250ms待つ
tomotsugu 0:5d6b3a226854 76 SR = watch();
tomotsugu 0:5d6b3a226854 77 if(SR < limit){ // 右20cm以内に障害物がある場合
tomotsugu 0:5d6b3a226854 78 run = STOP; // 停止
tomotsugu 0:5d6b3a226854 79 }
tomotsugu 0:5d6b3a226854 80 servo.pulsewidth_us(1450); // サーボを中央位置に戻す
tomotsugu 0:5d6b3a226854 81 thread_sleep_for(10); // 100ms待つ
tomotsugu 0:5d6b3a226854 82 if(SC < limit || SLD < limit || SL < limit || SRD < limit || SR < limit){ // 20cm以内に障害物を検知した場合
tomotsugu 0:5d6b3a226854 83 flag = 1; // フラグに1をセット
tomotsugu 0:5d6b3a226854 84 }
tomotsugu 0:5d6b3a226854 85 }
tomotsugu 0:5d6b3a226854 86 }
tomotsugu 0:5d6b3a226854 87
tomotsugu 0:5d6b3a226854 88 /* 障害物回避走行 */
tomotsugu 0:5d6b3a226854 89 void avoidance(){
tomotsugu 0:5d6b3a226854 90 while(1){
tomotsugu 0:5d6b3a226854 91 if(flag == 0){
tomotsugu 0:5d6b3a226854 92 run = ADVANCE;
tomotsugu 0:5d6b3a226854 93 }
tomotsugu 0:5d6b3a226854 94 else{
tomotsugu 0:5d6b3a226854 95 i = 0;
tomotsugu 0:5d6b3a226854 96 if(SC < 15){
tomotsugu 0:5d6b3a226854 97 run = BACK;
tomotsugu 0:5d6b3a226854 98 while(watch() < limit){
tomotsugu 0:5d6b3a226854 99 }
tomotsugu 0:5d6b3a226854 100 run = STOP;
tomotsugu 0:5d6b3a226854 101 }
tomotsugu 0:5d6b3a226854 102 if(SC < limit && SLD < limit && SL < limit && SRD < limit && SR < limit){
tomotsugu 0:5d6b3a226854 103 run = LEFT;
tomotsugu 0:5d6b3a226854 104 while(i < 100){
tomotsugu 0:5d6b3a226854 105 if(watch() > limit){
tomotsugu 0:5d6b3a226854 106 i++;
tomotsugu 0:5d6b3a226854 107 }
tomotsugu 0:5d6b3a226854 108 else{
tomotsugu 0:5d6b3a226854 109 i = 0;
tomotsugu 0:5d6b3a226854 110 }
tomotsugu 0:5d6b3a226854 111 }
tomotsugu 0:5d6b3a226854 112 run = STOP;
tomotsugu 0:5d6b3a226854 113 }
tomotsugu 0:5d6b3a226854 114 max = SC;
tomotsugu 0:5d6b3a226854 115 houkou = 1;
tomotsugu 0:5d6b3a226854 116 if(max < SLD || max < SL){
tomotsugu 0:5d6b3a226854 117 if(SL < SLD){
tomotsugu 0:5d6b3a226854 118 max = SLD;
tomotsugu 0:5d6b3a226854 119 }
tomotsugu 0:5d6b3a226854 120 else{
tomotsugu 0:5d6b3a226854 121 max = SL;
tomotsugu 0:5d6b3a226854 122 }
tomotsugu 0:5d6b3a226854 123 houkou = 2;
tomotsugu 0:5d6b3a226854 124 }
tomotsugu 0:5d6b3a226854 125 if(max < SRD || max < SR){
tomotsugu 0:5d6b3a226854 126 if(SR < SRD){
tomotsugu 0:5d6b3a226854 127 max = SRD;
tomotsugu 0:5d6b3a226854 128 }
tomotsugu 0:5d6b3a226854 129 else{
tomotsugu 0:5d6b3a226854 130 max = SR;
tomotsugu 0:5d6b3a226854 131 }
tomotsugu 0:5d6b3a226854 132 houkou = 3;
tomotsugu 0:5d6b3a226854 133 }
tomotsugu 0:5d6b3a226854 134 switch(houkou){
tomotsugu 0:5d6b3a226854 135 case 1:
tomotsugu 0:5d6b3a226854 136 run = ADVANCE;
tomotsugu 0:5d6b3a226854 137 break;
tomotsugu 0:5d6b3a226854 138 case 2:
tomotsugu 0:5d6b3a226854 139 run = LEFT;
tomotsugu 0:5d6b3a226854 140 while(i < 100){
tomotsugu 0:5d6b3a226854 141 if(watch() > (max - 2)){
tomotsugu 0:5d6b3a226854 142 i++;
tomotsugu 0:5d6b3a226854 143 }
tomotsugu 0:5d6b3a226854 144 else{
tomotsugu 0:5d6b3a226854 145 i = 0;
tomotsugu 0:5d6b3a226854 146 }
tomotsugu 0:5d6b3a226854 147 }
tomotsugu 0:5d6b3a226854 148 run = STOP;
tomotsugu 0:5d6b3a226854 149 break;
tomotsugu 0:5d6b3a226854 150 case 3:
tomotsugu 0:5d6b3a226854 151 run = RIGHT;
tomotsugu 0:5d6b3a226854 152 while(i < 100){
tomotsugu 0:5d6b3a226854 153 if(watch() > (max - 2)){
tomotsugu 0:5d6b3a226854 154 i++;
tomotsugu 0:5d6b3a226854 155 }
tomotsugu 0:5d6b3a226854 156 else{
tomotsugu 0:5d6b3a226854 157 i = 0;
tomotsugu 0:5d6b3a226854 158 }
tomotsugu 0:5d6b3a226854 159 }
tomotsugu 0:5d6b3a226854 160 run = STOP;
tomotsugu 0:5d6b3a226854 161 break;
tomotsugu 0:5d6b3a226854 162 }
tomotsugu 0:5d6b3a226854 163 }
tomotsugu 0:5d6b3a226854 164 flag = 0;
tomotsugu 0:5d6b3a226854 165 Thread::signal_set(0x1);
tomotsugu 0:5d6b3a226854 166 }
tomotsugu 0:5d6b3a226854 167 }
tomotsugu 0:5d6b3a226854 168