File content as of revision 3:2ae6218973be:

/* mbed Microcontroller Library
 * Copyright (c) 2019 ARM Limited
 * SPDX-License-Identifier: Apache-2.0
 */

#include "mbed.h"
#include "ReceiverIR.h"
#include "rtos.h"
#include <stdint.h>
#include "platform/mbed_thread.h"
#include "TextLCD.h"

Serial pc(USBTX, USBRX);

/* マクロ定義、列挙型定義 */
#define MIN_V 2.0
#define MAX_V 2.67
#define     LOW     0       // 
#define     HIGH    1       // 
#define     NORMAL   0       // フラグ値
#define     FAST   1       // フラグ値
#define     VERYFAST   2       // フラグ値

enum MODE{
    READY   = -1,
    ADVANCE = 1,
    RIGHT,
    LEFT,
    BACK,
    STOP,
    LINE_TRACE,
    AVOIDANCE,
    SPEED,
};

/* ピン配置 */
ReceiverIR   ir(p5);        // リモコン操作
DigitalOut  trig(p6);       // 超音波センサtrigger
DigitalOut  echo(p7);       // 超音波センサecho
DigitalIn   ss1(p8);        // ライントレースセンサ(左)
DigitalIn   ss2(p9);        // ライントレースセンサ
DigitalIn   ss3(p10);       // ライントレースセンサ
DigitalIn   ss4(p11);       // ライントレースセンサ
DigitalIn   ss5(p12);       // ライントレースセンサ(右)
Serial      esp(p13, p14);  // Wi-Fiモジュール(tx, rx)
AnalogIn    battery(p15);   // 電池残量読み取り(Max 3.3V)
PwmOut      motorR2(p21);   // 右モーター後退
PwmOut      motorR1(p22);   // 右モーター前進
PwmOut      motorL2(p23);   // 左モーター後退
PwmOut      motorL1(p24);   // 左モーター前進
PwmOut      servo(p25);     // サーボ
I2C         i2c_lcd(p28,p27);   // LCD(tx, rx)

/* 変数宣言 */
int mode;                   // 操作モード
int run;                    // 走行状態
int beforeMode;             // 前のモード
int flag_sp = 0;             // スピード変化フラグ
Timer viewTimer;            // スピ―ド変更時に3秒計測
float motorSpeed[6] = {0.7, 0.8, 0.9, 0.8, 0.9, 1.0};
                            // モーター速度設定(後半はライントレース用)
Mutex  mutex;                       
// ポートp15を赤外線受信モジュールの接続先に指定
RemoteIR::Format format;
uint8_t buf[32];
uint32_t bitcount;
uint32_t code;

//bChangeのいろいろ
TextLCD_I2C lcd(&i2c_lcd, (0x27 << 1), TextLCD::LCD16x2, TextLCD::HD44780);
int b = 0;
double test;
int flag_b = 0;
int flag_t = 0;

// ライントレース
int sensArray[32] = {0,0,0,0,0,0,0,0,   // ライントレースセンサパターン
                    0,0,0,0,0,0,0,0,
                    0,0,0,0,0,0,0,0,
                    0,0,0,0,0,0,0,0};

/* 障害物検知用の変数設定 */
Timer timer;            // 距離計測用タイマ
int DT;                 // 距離
int SC;                 // 正面   
int SL;                 // 左
int SR;                 // 右
int SLD;                // 左前
int SRD;                // 右前
int flag = 0;           // 障害物有無のフラグ
const int limit = 20;   // 障害物の距離のリミット(単位：cm)
int far;                // 最も遠い距離
int houkou;             // 進行方向(1:前 2:左 3:右)
int i;                  // ループ変数



/* プロトタイプ宣言 */
void decodeIR(void const *argument);
void motor(void const *argument);
void changeSpeed();
void avoidance(void const *argument);
void trace(void const *argument);
void watchsurrounding();
int watch();
void bChange(void const *argument);
void display();
void lcdBacklight(void const *argument);
Thread thread1(decodeIR, NULL, osPriorityAboveNormal);
Thread thread2(motor, NULL, osPriorityNormal);
Thread thread3(avoidance, NULL, osPriorityLow);
Thread thread4(trace, NULL, osPriorityLow);
Thread thread5(bChange, NULL, osPriorityIdle);
RtosTimer bTimer(lcdBacklight, osTimerPeriodic);


void decodeIR(void const *argument){  
    while(1){  
        // 受信待ち
        if (ir.getState() == ReceiverIR::Received) {
            // コード受信
            bitcount = ir.getData(&format, buf, sizeof(buf) * 8);
            if(bitcount > 1){
                // 受信成功
                code=0;
                for(int j=0;j<4;j++){
                    code+=(buf[j]<<(8*(3-j)));
                }
                if(mode!=SPEED){
                    beforeMode=mode;
                }
                
                //pc.printf("%0x\r\n",code);
                switch(code){
                    case 0x40bf27d8://クイック
                        pc.printf("mode = SPEED\r\n");
                        mode = SPEED;
                        break;
                    case 0x40be34cb://レグザリンク
                        pc.printf("mode = INE_TRACE\r\n");
                        mode=LINE_TRACE;
                        display();
                        break;
                    case 0x40bf6e91://番組表
                        pc.printf("mode = AVOIDANCE\r\n");
                        mode=AVOIDANCE;
                        display();
                        break;
                    case 0x40bf3ec1://↑
                        pc.printf("mode = ADVANCE\r\n");
                        mode = ADVANCE;
                        run = ADVANCE;
                        display();
                        break;
                    case 0x40bf3fc0://↓
                        pc.printf("mode = BACK\r\n");
                        mode = BACK;
                        run = BACK;
                        display();
                        break;
                    case 0x40bf5fa0://←
                        pc.printf("mode = LEFT\r\n");
                        mode = LEFT;
                        run = LEFT;
                        display();
                        break;
                    case 0x40bf5ba4://→
                        pc.printf("mode = RIGHT\r\n");
                        mode = RIGHT;
                        run = RIGHT;
                        display();
                        break;
                    case 0x40bf3dc2://決定
                        pc.printf("mode = STOP\r\n");
                        mode = STOP;
                        run = STOP;
                        display();
                        break;
                    default:
                        ;
                }
            }
        }
        ThisThread::sleep_for(10);
    }        
}
void motor(void const *argument){
    while(true){
        pc.printf("motor\r\n");
        if(flag_sp >VERYFAST){
            flag_sp -= 3;
        }
        switch(run){
            case ADVANCE:
                motorR1 = motorSpeed[flag_sp];
                motorR2 = LOW;
                motorL1 = motorSpeed[flag_sp];
                motorL2 = LOW;
                break;
            case RIGHT:
                motorR1 = LOW;
                motorR2 = motorSpeed[flag_sp];
                motorL1 = motorSpeed[flag_sp];
                motorL2 = LOW;
                break;
            case LEFT:
                motorR1 = motorSpeed[flag_sp];
                motorR2 = LOW;
                motorL1 = LOW;
                motorL2 = motorSpeed[flag_sp];
                break;
            case BACK:
                motorR1 = LOW;
                motorR2 = motorSpeed[flag_sp];
                motorL1 = LOW;
                motorL2 = motorSpeed[flag_sp];
                break;
            case STOP:
                motorR1 = LOW;
                motorR2 = LOW;
                motorL1 = LOW;
                motorL2 = LOW;
                break;
        }
        pc.printf("                motor\r\n");
        ThisThread::sleep_for(50);
    }
}
void changeSpeed(){
    if(flag_sp == VERYFAST){
        flag_sp = NORMAL;
        
    }else{
        flag_sp = flag_sp + 1;   
    }  
}
void trace(void const *argument){
    while(true){ 
        if(mode==LINE_TRACE){        
            pc.printf("line trace\r\n");
            // センサー値読み取り
            int sensor1 = ss1;
            int sensor2 = ss2;
            int sensor3 = ss3;
            int sensor4 = ss4;
            int sensor5 = ss5;
        
            int sensD = 0;
            int sensorNum;
        
            if(sensor1 > 0) sensD |= 0x10;
            if(sensor2 > 0) sensD |= 0x08;
            if(sensor3 > 0) sensD |= 0x04;
            if(sensor4 > 0) sensD |= 0x02;
            if(sensor5 > 0) sensD |= 0x01;
    
            sensorNum = sensArray[sensD];
    
            switch(sensorNum){
                case 1:
                    run = ADVANCE;
                    break;
                case 2:
                    run = RIGHT;
                    break;
                case 3:
                    run = LEFT;
                    break;
                case 4:
                    flag_sp += 3;
                    run = RIGHT;
                    break;
                case 5:
                    flag_sp += 3;
                    run = LEFT;
                    break;
            }
            pc.printf("                          line trace\r\n");
            ThisThread::sleep_for(50);
        }else{          
            ThisThread::sleep_for(50);
        }
    }
}

/* 障害物回避走行 */
void avoidance(void const *argument){
    while(1){  
        if(mode==AVOIDANCE){
            pc.printf("avoidance\r\n"); 
            if(flag == 0){                      // 障害物がない場合
                run = ADVANCE;                  // 前進
            }
            /*else{                               // 障害物がある場合                   
                i = 0;
                if(SC < 15){                    // 正面15cm以内に障害物が現れた場合
                    run = BACK;                 // 後退
                    while(watch() < limit){     // 正面20cm以内に障害物がある間
                    
                    }
                    run = STOP;                 // 停止                 
                }
                if(SC < limit && SLD < limit && SL < limit && SRD < limit && SR < limit){   // 全方向に障害物がある場合
                    run = LEFT;                 // 左折                   
                    while(i < 100){             // 進行方向確認
                        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;                  // 前進
                            thread_sleep_for(100);          // 1秒待つ
                            break;
                        case 2:                             // 左の場合
                            run = LEFT;                     // 左折
                            while(i < 100){                 // 進行方向確認
                                if(watch() > (far - 2)){    
                                    i++;
                                }
                                else{
                                    i = 0;
                                }
                            }
                            run = STOP;                     // 停止
                            break;
                        case 3:                             // 右の場合
                            run = RIGHT;                    // 右折
                            while(i < 100){                 // 進行方向確認
                                if(watch() > (far - 2)){
                                    i++;
                                }
                                else{
                                    i = 0;
                                }
                            }
                            run = STOP;                     // 停止
                            break;
                    }
                }
            }*/
            flag = 0;                   // フラグを0にセット
            watchsurrounding();
            pc.printf("                avoidance\r\n");
        }else{  
            ThisThread::sleep_for(50);
        }
    }   
}
int watch(){
    pc.printf("watch\r\n");
    trig = 0;
    ThisThread::sleep_for(5);           // 5ms待つ
    trig = 1;
    ThisThread::sleep_for(15);          // 15ms待つ
    trig = 0;
    //while(echo.read() == 0){
    //}
    timer.start();                  // 距離計測タイマースタート
    while(echo.read() == 1){
    }
    timer.stop();                   // 距離計測タイマーストップ
    DT = timer.read_us()*0.01657;   // 距離計算
    if(DT > 400){                   // 検知範囲外なら400cmに設定
        DT = 400;
    }
    timer.reset();                  // 距離計測タイマーリセット
    //ThisThread::sleep_for(30);      // 30ms待つ
    pc.printf("                watch\r\n");
    return DT;
}

void watchsurrounding(){
    pc.printf("watchsurrounding\r\n");
    SC = watch();
    if(SC < limit){                 // 正面20cm以内に障害物がある場合
        run = STOP;                 // 停止
    }
    servo.pulsewidth_us(1925);      // サーボを左に40度回転
    ThisThread::sleep_for(250);           // 250ms待つ
    SLD = watch();
    if(SLD < limit){                // 左前20cm以内に障害物がある場合
        run = STOP;                 // 停止
    }
    servo.pulsewidth_us(2400);      // サーボを左に90度回転
    ThisThread::sleep_for(250);           // 250ms待つ
    SL = watch();
    if(SL < limit){                 // 左20cm以内に障害物がある場合
        run = STOP;                 // 停止
    }
    servo.pulsewidth_us(1450);
    ThisThread::sleep_for(100);
    servo.pulsewidth_us(925);       // サーボを右に40度回転
    ThisThread::sleep_for(250);           // 250ms待つ
    SRD = watch();
    if(SRD < limit){                // 右前20cm以内に障害物がある場合
        run = STOP;                 // 停止
    }
    servo.pulsewidth_us(500);       // サーボを右に90度回転
    ThisThread::sleep_for(250);           // 250ms待つ
    SR = watch();
    if(SR < limit){                 // 右20cm以内に障害物がある場合
        run = STOP;                 // 停止
    }
    servo.pulsewidth_us(1450);      // サーボを中央位置に戻す
    ThisThread::sleep_for(100);           // 100ms待つ
    if(SC < limit || SLD < limit || SL < limit || SRD < limit || SR < limit){ // 20cm以内に障害物を検知した場合
        flag = 1;                   // フラグに1をセット
    }
    pc.printf("                watchsurrounding\r\n");
}

void defaultView(){
    lcd.setAddress(0,0);
    lcd.printf("Battery:");
    lcd.setAddress(0,1);
    lcd.printf("Mode:");
}

void display(){
        mutex.lock();
        lcd.setAddress(0,1);
        switch(mode){
            case ADVANCE:
                //strcpy(DispMode,"Mode:Advance");
                lcd.printf("Mode:Advance    ");
                break;
            case RIGHT:
                //strcpy(DispMode,"Mode:Right");
                lcd.printf("Mode:Right      ");
                break;
            case LEFT:
                //strcpy(DispMode,"Mode:Left");
                lcd.printf("Mode:Left       ");
                break;
            case BACK:
                //strcpy(DispMode,"Mode:Back");
                lcd.printf("Mode:Back       ");
                break;
            case STOP:
                //strcpy(DispMode,"Mode:Stop");
                lcd.printf("Mode:Stop       ");
                break;
            case READY:
                //strcpy(DispMode,"Mode:Stop");
                lcd.printf("Mode:Ready      ");
                break;
            case LINE_TRACE:
                //strcpy(DispMode,"Mode:LineTrace");
                lcd.printf("Mode:LineTrace  ");
                break;
            case AVOIDANCE:
                //strcpy(DispMode,"Mode:Avoidance");
                lcd.setAddress(0,1);
                lcd.printf("Mode:Avoidance  ");
                break;
            case SPEED:
                switch(flag_sp){
                    case(NORMAL):
                        lcd.printf("Speed:Normal    ");
                        break;
                    case(FAST):
                        lcd.printf("Speed:Fast      ");
                        break;
                    case(VERYFAST):
                        lcd.printf("Speed:VeryFast  ");
                        break;
                }
                viewTimer.reset();
                viewTimer.start();  
                break;              
        }
        mutex.unlock(); 
}

void lcdBacklight(void const *argument){
    if(flag_b == 1){
        lcd.setBacklight(TextLCD::LightOn);
    }else{
        lcd.setBacklight(TextLCD::LightOff);
    }
    flag_b = !flag_b;
}

void bChange(void const *argument){
    /*while(1){
        pc.printf("                                                                              bChange1\r\n");
        lcd.setBacklight(TextLCD::LightOn);
        test = battery.read()*MAX_V;
        b = (int)((test - MIN_V)/0.107 + 0.5)*10;
        if(b < 0){
            b = 0;
        }else if(b > 100){
            b = 100;
        }
        mutex.lock();
        lcd.setAddress(0,0);
        lcd.printf("Battery:%3d%%    ",b);
        pc.printf("                                                                              bChange2\r\n");
        mutex.unlock();
    }*/
    lcd.setBacklight(TextLCD::LightOn);
    while(1){
        pc.printf("                                                                              bChange1\r\n");
        b = (int)((battery.read()* MAX_V - MIN_V)/0.067 + 0.5)*10;
        if(b < 0){//すべての機能停止
            lcd.setBacklight(TextLCD::LightOff);
            bTimer.stop();
            exit(1);
        }else if(b > 100){
            b = 100;
        }
        mutex.lock();
        lcd.setAddress(0,0);
        lcd.printf("Battery:%3d%%",b);
        pc.printf("                                                                              bChange2\r\n");
        mutex.unlock();
        if(b <= 30){
            if(flag_t == 0){
                bTimer.start(500);
                flag_t = 1;
            }
        }else{
            if(flag_t == 1){
                bTimer.stop();
                lcd.setBacklight(TextLCD::LightOn);
                flag_t = 0;
            }
        }
    }
        
}
int main() {
    mode = READY;
    beforeMode = READY;
    run = STOP;
    flag_sp = NORMAL;
    //defaultView();
    
    while(1){
        pc.printf("                                    main1\r\n");
        switch(mode){
            /*case ADVANCE:
                run = ADVANCE;
                break;
            case RIGHT:
                run = RIGHT;
                break;
            case LEFT:
                run = LEFT;
                break;
            case BACK:
                run = BACK;
                break;
            case STOP:
                run = STOP;
                break;
            case READY:
                run = STOP;
                break;*/
            case SPEED:
                changeSpeed();
                display();  
                mode = beforeMode;
                break;            
        }

        if(viewTimer.read_ms()>=3000){
            pc.printf("a");
            viewTimer.stop();
            //defaultView();
            viewTimer.reset();
            //flaga=0;
        }
        pc.printf("                                    main2\r\n");
        if(viewTimer.read_ms()==0){
            display();
        }
        pc.printf("                                    main3\r\n");
        ThisThread::sleep_for(10);
    }
}