denki1
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){