1
Dependencies: RemoteIR TextLCD linetrace
Revision 52:b599247d4a02, committed 2020-09-23
- Comitter:
- jiang111
- Date:
- Wed Sep 23 02:09:32 2020 +0000
- Parent:
- 51:2d35f207d217
- Commit message:
- 1
Changed in this revision
linetrace1.lib | Show annotated file Show diff for this revision Revisions of this file |
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 2d35f207d217 -r b599247d4a02 linetrace1.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/linetrace1.lib Wed Sep 23 02:09:32 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/denki/code/linetrace/#2d35f207d217
diff -r 2d35f207d217 -r b599247d4a02 main.cpp --- a/main.cpp Tue Sep 01 08:00:17 2020 +0000 +++ b/main.cpp Wed Sep 23 02:09:32 2020 +0000 @@ -1,3 +1,4 @@ + /* mbed Microcontroller Library * Copyright (c) 2019 ARM Limited * SPDX-License-Identifier: Apache-2.0 @@ -18,32 +19,32 @@ #define LOW 0 // モーターOFF #define HIGH 1 // モーターON #define NORMAL 0 // 普通 -#define FAST 1 // 速い -#define VERYFAST 2 // とても速い +#define FAST 3 // 速い +#define VERYFAST 6 // とても速い #define SLOW 0.8 #define REVERSE 0.5 #define MBED04 0.781//Mbed04号機の左右のモーター速度比(R:L = 1: 0.781) #define MBED05 0.953//Mbed05号機の左右のモーター速度比(R:L = 1: 0.953) -#define MSR0 0.4 -#define MSR1 0.5 -#define MSR2 0.6 -#define MSR3 MSR0*SLOW -#define MSR4 MSR1*SLOW -#define MSR5 MSR2*SLOW -#define MSR6 0.7 -#define MSR7 0.8 -#define MSR8 0.9 -#define MSL0 MSR0*MBED05 -#define MSL1 MSR1*MBED05 -#define MSL2 MSR2*MBED05 -#define MSL3 MSR3*MBED05 -#define MSL4 MSR4*MBED05 -#define MSL5 MSR5*MBED05 -#define MSL6 MSR6*MBED05 -#define MSL7 MSR7*MBED05 -#define MSL8 MSR8*MBED05 +#define MSR0 0.65 +#define MSR1 0.3 +#define MSR2 0.7 +#define MSR3 0.75 +#define MSR4 0.7 +#define MSR5 0.85 +#define MSR6 0.9 +#define MSR7 0.95 +#define MSR8 0.7 +#define MSL0 MSR0 +#define MSL1 MSR1 +#define MSL2 MSR2//*MBED05 +#define MSL3 MSR3//*MBED05 +#define MSL4 MSR4//*MBED05 +#define MSL5 MSR5//*MBED05 +#define MSL6 MSR6//*MBED05 +#define MSL7 MSR7//*MBED05 +#define MSL8 MSR8//*MBED05 /* 操作モード定義 */ enum MODE{ @@ -76,6 +77,8 @@ PwmOut servo(p25); // サーボ I2C i2c_lcd(p28,p27); // LCD(tx, rx) + + /* 変数宣言 */ int mode; // 操作モード int run; // 走行状態 @@ -86,10 +89,10 @@ float motorSpeedR1[9] = {MSR0, MSR1, MSR2, MSR3 , MSR4 , MSR5 , MSR6 , MSR7 , MSR8 }; //float motorSpeedR2[9] = {MSR0, MSR1, MSR2, MSR3*REVERSE, MSR4*REVERSE, MSR5*REVERSE, MSR6*REVERSE, MSR7*REVERSE, MSR8*REVERSE}; -float motorSpeedR2[9] = {MSR0, MSR1, MSR2, MSR3*REVERSE, MSR4*REVERSE, MSR5*REVERSE, MSR6, MSR7, MSR8}; +float motorSpeedR2[9] = {MSR0, MSR1, MSR2, MSR3, MSR4, MSR5, MSR6, MSR7, MSR8}; float motorSpeedL1[9] = {MSL0, MSL1, MSL2, MSL3 , MSL4 , MSL5 , MSL6 , MSL7 , MSL8 }; //float motorSpeedL2[9] = {MSL0, MSL1, MSL2, MSL3*REVERSE, MSL4*REVERSE, MSL5*REVERSE, MSL6*REVERSE, MSL7*REVERSE, MSL8*REVERSE}; -float motorSpeedL2[9] = {MSL0, MSL1, MSL2, MSL3*REVERSE, MSL4*REVERSE, MSL5*REVERSE, MSL6, MSL7, MSL8}; +float motorSpeedL2[9] = {MSL0, MSL1, MSL2, MSL3, MSL4, MSL5, MSL6, MSL7, MSL8}; // モーター速度設定(後半はライントレース用) // 0,1,2:基準速度 // 3,4,5:低速 @@ -140,7 +143,7 @@ int SLD; // 左前 int SRD; // 右前 int flag_a = 0; // 障害物有無のフラグ -const int limit = 20; // 障害物の距離のリミット(単位:cm) +const int limit = 25; // 障害物の距離のリミット(単位:cm) int far; // 最も遠い距離 int houkou; // 進行方向(1:前 2:左 3:右) int t1 = 0; @@ -196,7 +199,7 @@ deco_thread -> set_priority(osPriorityRealtime); motor_thread = new Thread(motor); motor_thread -> set_priority(osPriorityHigh); - wifi_thread -> set_priority(osPriorityRealtime); + //wifi_thread -> set_priority(osPriorityRealtime); display(); } @@ -310,16 +313,18 @@ /* 右折 */ case RIGHT: motorR1 = LOW; // 右前進モーターOFF - motorR2 = motorSpeedR2[flag_sp]; // 右後退モーターON - motorL1 = motorSpeedL1[flag_sp]; // 左前進モーターON + motorR2 = motorSpeedR2[8]; // 右後退モーターON + motorL1 = motorSpeedL1[8]; // 左前進モーターON motorL2 = LOW; // 左後退モーターOFF + break; /* 左折 */ case LEFT: - motorR1 = motorSpeedR1[flag_sp]; // 右前進モーターON + motorR1 = motorSpeedR1[8]; // 右前進モーターON motorR2 = LOW; // 右後退モーターOFF motorL1 = LOW; // 左前進モーターOFF - motorL2 = motorSpeedL2[flag_sp]; // 左後退モーターON + motorL2 = motorSpeedL2[8]; // 左後退モーターON + break; /* 後退 */ case BACK: @@ -328,6 +333,18 @@ motorL1 = LOW; // 左前進モーターOFF motorL2 = motorSpeedL2[flag_sp]; // 左後退モーターON break; + case 11: + motorR1 = LOW; // 右前進モーターOFF + motorR2 = motorSpeedR2[4]; // 右後退モーターON + motorL1 = LOW; // 左前進モーターOFF + motorL2 = motorSpeedL2[1]; // 左後退モーターON + break; + case 21: + motorR1 = LOW; // 右前進モーターOFF + motorR2 = motorSpeedR2[1]; // 右後退モーターON + motorL1 = LOW; // 左前進モーターOFF + motorL2 = motorSpeedL2[4]; // 左後退モーターON + break; /* 停止 */ case STOP: motorR1 = LOW; // 右前進モーターOFF @@ -345,11 +362,9 @@ /* スピード変更関数 */ void changeSpeed(){ - if(flag_sp%3 == 2){ // スピード変更フラグを3で割った余りが2なら - flag_sp = 0; // スピード変更フラグを0にする - }else{ // それ以外 - flag_sp = flag_sp + 1; // スピード変更フラグを+1 - } + // それ以外 + flag_sp = flag_sp + 3; // スピード変更フラグを+1 + if(flag_sp>6)flag_sp=0; } /* ライントレーススレッド */ @@ -363,7 +378,6 @@ int sensor5 = ss5; ////*-*-*-5("%d %d %d %d %d \r\n",sensor1,sensor2,sensor3,sensor4,sensor5); int sensD = 0; - /* センサー値の決定 */ if(sensor1 > 0) sensD |= 0x10; if(sensor2 > 0) sensD |= 0x08; @@ -417,133 +431,68 @@ } /* 障害物回避走行スレッド */ -void avoidance(){ +void avoidance() +{ int i; - while(1){ - watchsurrounding3(); -// //*-*-*-5("%d %d %d %d %d \r\n",SL,SLD,SC,SRD,SR); - if(flag_a == 0){ // 障害物がない場合 - run = ADVANCE; // 前進 - }else{ // 障害物がある場合 - i = 0; - if(SC < 15){ // 正面15cm以内に障害物が現れた場合 - servo.pulsewidth_us(1450); // サーボを中央位置に戻す - ThisThread::sleep_for(100); // 100ms待つ - run = BACK; // 後退 - int cnt_kyori=0; - int kyori = watch(); - while(kyori < limit){ // 正面20cm以内に障害物がある間 - if(kyori==-1){ - cnt_kyori++; - if(cnt_kyori>15){ - cnt_kyori=0; - break; - } - } - kyori = watch(); - } - /*while(i < 30){ // 正面20cm以内に障害物がある間 - if(watch() < limit){ - break; - } - i++; - } - i = 0;*/ - run = STOP; // 停止 - } - watchsurrounding5(); - if(SC < limit && SLD < limit && SL < limit && SRD < limit && SR < limit){ // 全方向に障害物がある場合 - run = LEFT; // 左折 - while(i < 1){ // 進行方向確認 - if(watch() > limit){ - i++; - }else{ - i = 0; - } - } - run = STOP; // 停止 - }else { // 全方向以外 - far = SC; // 正面を最も遠い距離に設定 - houkou = 1; // 進行方向を前に設定 - if(far < SLD || far < SL){ // 左または左前がより遠い場合 - if(SL < SLD){ // 左前が左より遠い場合 - far = SLD; // 左前を最も遠い距離に設定 - }else{ // 左が左前より遠い場合 - far = SL; // 左を最も遠い距離に設定 - } - houkou = 2; // 進行方向を左に設定 - } - if(far < SRD || far < SR){ // 右または右前がより遠い場合 - if(SR < SRD){ // 右前が右より遠い場合 - far = SRD; // 右前を最も遠い距離に設定 - }else{ // 右が右前よりも遠い場合 - far = SR; // 右を最も遠い距離に設定 - } - houkou = 3; // 進行方向を右に設定 - } - switch(houkou){ // 進行方向の場合分け - case 1: // 前の場合 - run = ADVANCE; // 前進 - ThisThread::sleep_for(500); // 0.5秒待つ - break; - case 2: // 左の場合 - run = LEFT; // 左折 - //int kyori = watch(); - //int kyori_f=0; - while(i < 20){ // 進行方向確認 - /*if(kyori > (far - 2) || kyori_f == 2){ // 正面の計測距離と最も遠い距離が一致したら(誤差-2cm) - break; // ループ+ - }else if(kyori==-1){ - kyori_f++; - }else{ - kyori_f = 0; - i++; - }*/ - if(watch() > (far - 2)){ // 正面の計測距離と最も遠い距離が一致したら(誤差-2cm) - break; // ループ+ + watchsurrounding3(); + while(1){ + if(flag_a==0){run=ADVANCE;watchsurrounding3();} + else{i=0; + if(flag_a==2){ + servo.pulsewidth_us(1925); + houkou=3;} + + if(flag_a==3){servo.pulsewidth_us(925); + houkou=4;} + + if(flag_a==1){watchsurrounding5(); + if(SLD<=SC){houkou=3;} + if(SRD<=SC){houkou=4;} + if(SLD>SC&&SRD>SC){ + if(SL>=SR){houkou=1;} + if(SL<SR){houkou=2;}}} + + switch(houkou){ + case 1: + run = BACK; + ThisThread::sleep_for(150); + run = LEFT; + ThisThread::sleep_for(350); + break; + case 2: + run = BACK; + ThisThread::sleep_for(150); + run = RIGHT; + ThisThread::sleep_for(350); + break; + case 3: + run=11; + while(i < 20){ + + if(watch() > 25){ + break; }else{ i++; } } - run = STOP; // 停止 + run = STOP; break; - case 3: // 右の場合 - run = RIGHT; // 右折 - //int kyori = watch(); - //int kyori_f=0; - while(i < 20){ // 進行方向確認 - /*if(kyori > (far - 2) || kyori_f == 2){ // 正面の計測距離と最も遠い距離が一致したら(誤差-2cm) - break; // ループ+ - }else if(kyori==-1){ - kyori_f++; - }else{ - kyori_f = 0; - i++; - }*/ - if(watch() > (far - 2)){ // 正面の計測距離と最も遠い距離が一致したら(誤差-2cm) - break; // ループ+ + case 4: + run=21; + while(i < 20){ + + if(watch() > 25){ + break; }else{ i++; } } - run = STOP; // 停止 + run = STOP; break; - } - } + } + flag_a=0;} } - flag_a = 0; // 障害物有無フラグを0にセット - if(SLD < 29){ // 正面15cm以内に障害物が現れた場合 - run = RIGHT; // 右折 - ThisThread::sleep_for(200); // 100ms待つ - run = STOP; // 停止 - }else if(SRD < 29){ - run = LEFT; // 左折 - ThisThread::sleep_for(200); // 100ms待つ - run = STOP; // 停止 - } - } } - /* 距離計測関数 */ int watch(){ do{ @@ -583,7 +532,7 @@ //servo.pulsewidth_us(1450); // サーボを中央位置に戻す //ThisThread::sleep_for(200); // 100ms待つ SC = watch(); - if(SC < limit){ // 正面20cm以内に障害物がある場合 + if(SC < 25){ // 正面20cm以内に障害物がある場合 if(SC!=-1){ run = STOP; // 停止 flag_a = 1; @@ -593,15 +542,15 @@ servo.pulsewidth_us(1925); // サーボを左に40度回転 ThisThread::sleep_for(100); // 250ms待つ SLD = watch(); - if(SLD < limit){ // 左前20cm以内に障害物がある場合 + if(SLD < 20){ // 左前20cm以内に障害物がある場合 run = STOP; // 停止 - flag_a = 1; + flag_a = 2; return; } servo.pulsewidth_us(1450); ThisThread::sleep_for(150); SC = watch(); - if(SC < limit){ + if(SC < 25){ if(SC!=-1){ run = STOP; // 停止 flag_a = 1; @@ -611,9 +560,9 @@ servo.pulsewidth_us(925); // サーボを右に40度回転 ThisThread::sleep_for(100); // 250ms待つ SRD = watch(); - if(SRD < limit){ // 右前20cm以内に障害物がある場合 + if(SRD < 20){ // 右前20cm以内に障害物がある場合 run = STOP; // 停止 - flag_a = 1; + flag_a = 3; return; } servo.pulsewidth_us(1450); // サーボを中央位置に戻す @@ -625,8 +574,8 @@ /* 障害物検知関数 */ void watchsurrounding5(){ - //servo.pulsewidth_us(1450); // サーボを中央位置に戻す - //ThisThread::sleep_for(200); // 100ms待つ + servo.pulsewidth_us(1450); // サーボを中央位置に戻す + ThisThread::sleep_for(200); // 100ms待つ SC = watch(); servo.pulsewidth_us(1925); // サーボを左に40度回転 ThisThread::sleep_for(100); // 250ms待つ @@ -689,7 +638,7 @@ /* スピード制御 */ case SPEED: /* スピードの状態で場合分け */ - switch(flag_sp % 3){ + switch(flag_sp){ /* 普通 */ case(NORMAL): lcd.printf("Speed:Normal "); @@ -1005,7 +954,6 @@ strcat(webbuff, "for(var m=0;m<11;m++){button_9[m].disabled=true;}"); strcat(webbuff, "var xhr = new XMLHttpRequest();"); strcat(webbuff, "xhr.open(\"GET\", url);"); - strcat strcat(webbuff, "xhr.onreadystatechange = function(){"); strcat(webbuff, "if(this.readyState == 4 || this.status == 200){"); strcat(webbuff, "for(var m=0;m<11;m++){button_9[m].disabled=false;}"); @@ -1366,9 +1314,13 @@ /* mainスレッド */ int main() { /* 初期設定 */ - wifi_thread = new Thread(wifi); - wifi_thread -> set_priority(osPriorityHigh); -// setup(); + //wifi_thread = new Thread(wifi); + //wifi_thread -> set_priority(osPriorityHigh); + /* motorR2.period_us(40); + motorR1.period_us(40); + motorL2.period_us(40); + motorL1.period_us(40);*/ + setup(); avoi_thread = new Thread(avoidance); avoi_thread->terminate(); trace_thread = new Thread(trace); @@ -1379,7 +1331,7 @@ flag_sp = NORMAL; // スピード(普通) lcd.setBacklight(TextLCD::LightOn); // バックライトON lcd.setAddress(0,1); - lcd.printf("Mode:SetUp"); + lcd.printf("Mode:ready"); // display(); // ディスプレイ表示 while(1){