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@1:9787fdc9b191, 2020-07-22 (annotated)

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