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@9:5f197f0fa9bd, 2020-07-27 (annotated)

Committer:
nishimura_taku_pet
Date:
Mon Jul 27 01:43:23 2020 +0000
Revision:
9:5f197f0fa9bd
Parent:
8:6320ca55f5d8
Child:
10:8f933ed27db0
commit

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