linetrace saikyo!

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 /* mbed Microcontroller Library
00002  * Copyright (c) 2019 ARM Limited
00003  * SPDX-License-Identifier: Apache-2.0
00004  */
00005 
00006 #include "mbed.h"
00007 #include "platform/mbed_thread.h"
00008 #include <stdio.h>
00009 #include <stdlib.h>
00010 
00011 /* マクロ定義、列挙型定義 */
00012 #define     LOW         0
00013 
00014 // 使うロボットカーに合わせる
00015 #define     MBED01      1
00016 #define     MBED09      0.99
00017 #define     MBED        MBED09      //使うロボットカーの番号にする
00018 
00019 // 0: 前進  1: 旋回  2: 速度差  3: 障害物回避後戻る
00020 float motorSpeedR1[9] = {0.5     , 0.5     , 0.5     , 0.5     };
00021 float motorSpeedR2[9] = {0.5     , 0.5     , 0.5     , 0.5     };
00022 float motorSpeedL1[9] = {0.5*MBED, 0.5*MBED, 0.5*MBED, 0.5*MBED};
00023 float motorSpeedL2[9] = {0.5*MBED, 0.5*MBED, 0.5*MBED, 0.5*MBED};
00024 
00025 /* trace用変数 */
00026 int sensArray[32] = {0,4,2,4,1,4,2,4,   // ライントレースセンサパターン
00027                      3,4,1,4,3,4,1,4,   // 0:前回動作継続
00028                      5,1,5,1,5,1,5,1,   // 1:高速前進
00029                      5,1,5,1,5,1,5,1};  // 2:左が速い前進
00030                                         // 3:右が速い前進
00031                                         // 4:右旋回
00032                                         // 5:左旋回
00033 
00034 /* 操作モード定義 */
00035 enum MODE{
00036     READY   = -1,   // -1:待ち
00037     ADVANCE = 1,    //  1:前進
00038     RIGHT,          //  2:右折
00039     LEFT,           //  3:左折
00040     BACK,           //  4:後退
00041     STOP,           //  5:停止
00042     LINE_TRACE,     //  6:ライントレース
00043     AVOIDANCE,      //  7:障害物回避
00044     SPEED,          //  8:スピード制御
00045     LT_R,           //  9:低速右折(ライントレース時)
00046     LT_L,           // 10:低速左折(ライントレース時)
00047     AVOID_R,        // 11:ライントレース+障害物回避右
00048     AVOID_L         // 12:ライントレース+障害物回避左
00049 };
00050 
00051 /* ピン配置 */
00052 DigitalOut  trig(p6);           // 超音波センサtrigger
00053 DigitalIn   echo(p7);           // 超音波センサecho
00054 PwmOut      servo(p25);         // サーボ
00055 
00056 // 2輪
00057 DigitalIn   ss1(p8);            // ライントレースセンサ(左)
00058 DigitalIn   ss2(p9);            // ライントレースセンサ
00059 DigitalIn   ss3(p10);           // ライントレースセンサ
00060 DigitalIn   ss4(p11);           // ライントレースセンサ
00061 DigitalIn   ss5(p12);           // ライントレースセンサ(右)
00062 // 2輪
00063 PwmOut      motorR2(p22);       // 右モーター後退
00064 PwmOut      motorR1(p21);       // 右モーター前進
00065 PwmOut      motorL2(p23);       // 左モーター後退
00066 PwmOut      motorL1(p24);       // 左モーター前進
00067 
00068 DigitalOut  led1(LED1);         // ボードのLED1(右)
00069 DigitalOut  led4(LED4);         // ボードのLED4(左)
00070 
00071 int run = 0;                    // 走行モード
00072 int speed = 0;                  // 走行スピード
00073 int dist = 1000;                // 障害物までの距離
00074 int avoid_flag = 0;             // 障害物検知したかどうかのフラグ
00075 
00076 // 障害物回避用変数
00077 Timer timer;                    // 距離計測用タイマ
00078 Timer t;                        // 回避時間計測用タイマ
00079 int DT = 10000;                 // 障害物までの距離
00080 int flag_a = 0;                 // 障害物有無のフラグ
00081 const int limit = 35;           // 障害物の距離の限界値(単位:cm)
00082 int t1 = 0;                     // 検知にかかった時間
00083 
00084 LocalFileSystem local("local"); // ファイルを扱うときに使う
00085 RawSerial pc(USBTX, USBRX);     // PCとシリアル通信するときに使う
00086 
00087 void motor();                   // モーター制御関数
00088 int  watch();                   // 障害物検知関数
00089 void avoidance();               // 障害物回避関数
00090 
00091 void motor(){
00092     switch(run){
00093         // 前進
00094         case ADVANCE:
00095             motorR1 = motorSpeedR1[speed];  // 右前進モーターON
00096             motorR2 = LOW;                  // 右後退モーターOFF
00097             motorL1 = motorSpeedL1[speed];  // 左前進モーターON
00098             motorL2 = LOW;                  // 左後退モーターOFF
00099             break;
00100         // 右旋回
00101         case RIGHT:
00102             motorR1 = LOW;                  // 右前進モーターOFF
00103             motorR2 = motorSpeedR2[speed];  // 右後退モーターON
00104             motorL1 = motorSpeedL1[speed];  // 左前進モーターON
00105             motorL2 = LOW;                  // 左後退モーターOFF
00106             break;
00107         // 左旋回
00108         case LEFT:
00109             motorR1 = motorSpeedR1[speed];  // 右前進モーターON
00110             motorR2 = LOW;                  // 右後退モーターOFF
00111             motorL1 = LOW;                  // 左前進モーターOFF
00112             motorL2 = motorSpeedL2[speed];  // 左後退モーターON
00113             break;
00114         // 後退
00115         case BACK:
00116             motorR1 = LOW;                  // 右前進モーターOFF
00117             motorR2 = motorSpeedR2[speed];  // 右後退モーターON
00118             motorL1 = LOW;                  // 左前進モーターOFF
00119             motorL2 = motorSpeedR2[speed];  // 左後退モーターON
00120             break;
00121         // 停止
00122         case STOP:
00123             motorR1 = LOW;                  // 右前進モーターOFF
00124             motorR2 = LOW;                  // 右後退モーターOFF
00125             motorL1 = LOW;                  // 左前進モーターOFF
00126             motorL2 = LOW;                  // 左後退モーターOFF
00127             break;
00128         // 前進(左が速い)
00129         case LT_R:
00130             motorR1 = motorSpeedR2[speed];  // 右前進モーターON
00131             motorR2 = LOW;                  // 右後退モーターOFF
00132             motorL1 = motorSpeedL1[speed];  // 左前進モーターON
00133             motorL2 = LOW;                  // 左後退モーターOFF
00134             break;
00135         // 前進(右が速い)
00136         case LT_L:
00137             motorR1 = motorSpeedR1[speed];  // 右前進モーターON
00138             motorR2 = LOW;                  // 右後退モーターOFF
00139             motorL1 = motorSpeedL2[speed];  // 左前進モーターON
00140             motorL2 = LOW;                  // 左後退モーターOFF
00141             break;
00142         // ライントレース+障害物回避右
00143         case AVOID_R:
00144             motorR1 = 0.5;                  // 右前進モーターON
00145             motorR2 = LOW;                  // 右後退モーターOFF
00146             motorL1 = 1;                    // 左前進モーターON
00147             motorL2 = LOW;                  // 左後退モーターOFF
00148             break;
00149         // ライントレース+障害物回避左
00150         case AVOID_L:
00151             motorR1 = 1;                    // 右前進モーターON
00152             motorR2 = LOW;                  // 右後退モーターOFF
00153             motorL1 = 0.5;                  // 左前進モーターON
00154             motorL2 = LOW;                  // 左後退モーターOFF
00155             break;
00156     }
00157 }
00158 
00159 void trace(){
00160     int sensor1 = ss1;
00161     int sensor2 = ss2;
00162     int sensor3 = ss3;
00163     int sensor4 = ss4;
00164     int sensor5 = ss5;
00165     int sensD = 0;
00166     
00167     /* センサー値の決定 */
00168     if(sensor1 > 0) sensD |= 0x10;
00169     if(sensor2 > 0) sensD |= 0x08;
00170     if(sensor3 > 0) sensD |= 0x04;
00171     if(sensor4 > 0) sensD |= 0x02;
00172     if(sensor5 > 0) sensD |= 0x01;
00173     
00174     /* センサー値によって場合分け */
00175     switch(sensArray[sensD]){
00176         case 1:
00177             run = ADVANCE;      // 高速で前進
00178             speed = 0;
00179             break;
00180         case 2:
00181             run = LT_R;         // 前進(左が速い)
00182             speed = 2;
00183             break;
00184         case 3:
00185             run = LT_L;         // 前進(右が速い)
00186             speed = 2;
00187             break;
00188         case 4:
00189             run = RIGHT;        // 右旋回
00190             speed = 1;
00191             break;
00192         case 5:
00193             run = LEFT;         // 左旋回
00194             speed = 1;
00195             break;
00196         default:
00197             break;              // 前回動作を継続
00198     }
00199     motor();
00200 }
00201 
00202 int watch(){
00203     do{
00204         do{
00205             trig = 0;
00206             ThisThread::sleep_for(1);       // 5ms待つ
00207             trig = 1;
00208             ThisThread::sleep_for(1);      // 15ms待つ
00209             trig = 0;
00210             timer.start();
00211             t1=timer.read_ms();
00212             while(echo.read() == 0 && t1<3){
00213                 t1=timer.read_ms();
00214             }
00215             timer.stop();
00216             timer.reset();
00217         }while(t1 >= 3);
00218         timer.start();                  // 距離計測タイマースタート
00219         while(echo.read() == 1){
00220         }
00221         timer.stop();                   // 距離計測タイマーストップ
00222         DT = (int)(timer.read_us()*0.01657);   // 距離計算
00223         if(timer.read_ms() > 1000){
00224             DT = 1000;
00225             break;
00226         }
00227     }while(DT > 1000);
00228     if(DT > 400){
00229         DT = 400;
00230     }
00231     timer.reset();
00232     return DT;
00233 }
00234 
00235 void avoidance(int c){
00236     dist = watch();
00237     if(dist <= limit){
00238         if(c == 0){
00239             // 左に回避
00240             run = AVOID_L;
00241             motor();
00242             ThisThread::sleep_for(300);
00243             speed = 3;
00244             run = LT_R;
00245             motor();
00246             ThisThread::sleep_for(500);
00247         }else{
00248             // 右に回避
00249             run = AVOID_R;
00250             motor();
00251             ThisThread::sleep_for(300);
00252             speed = 3;
00253             run = LT_L;
00254             motor();
00255             ThisThread::sleep_for(500);
00256         }
00257         avoid_flag = 1;
00258     }
00259 }
00260 int main(){
00261     // 初期設定
00262     motorR1.period_us(40);                      // PWM周期設定
00263     motorR2.period_us(40);                      // PWM周期設定
00264     motorL1.period_us(40);                      // PWM周期設定
00265     motorL2.period_us(40);                      // PWM周期設定
00266 //servo.period_us(40);                            // PWM周期設定
00267     avoid_flag = 0;                             // 障害物回避していない
00268 
00269     // ファイルに回避方向を保存して制御
00270     FILE *fp;
00271     const char* fname = "/local/avo.txt";       // ファイル名
00272     int c;                                      // 読み込んだデータの格納
00273     if(( fp = fopen(fname, "r") ) == NULL){
00274 //        pc.printf("can't open.\r\n");
00275         fclose(fp);
00276         exit(1);
00277     }
00278     c = fgetc(fp);                              // ファイルからデータ読み込み
00279     c -= '0';                                   // 文字を数字に変換
00280     fclose(fp);
00281 //    pc.printf("c = %d\r\n", c);
00282 //    pc.printf("c - '0' = %d\r\n", c - '0');
00283     if(( fp = fopen(fname, "w") ) == NULL ){
00284         fclose(fp);
00285         exit(1);
00286     }
00287     if(c == 0){                                 // 読み込んだデータが 0 なら 1 を書き込み
00288         fputc('1', fp);
00289         led4 = 1;
00290 //        pc.printf("1\r\n");
00291     }else{                                      // 読み込んだデータが 1 なら 0 を書き込み
00292         fputc('0', fp);
00293         led1 = 1;
00294 //        pc.printf("0\r\n");
00295     }
00296     fclose(fp);
00297     
00298     while (true) {
00299         trace();
00300         if(avoid_flag == 0){                    // まだ障害物を検知していなければ
00301             avoidance(c);
00302         }
00303     }
00304 }