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

--- /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

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