mbed_robotcar
linetrace saikyo!
main.cpp
- Committer:
- takuminomura
- Date:
- 2020-09-24
- Revision:
- 2:cc4e4527ea43
- Parent:
- 1:fd7dd12feee0
File content as of revision 2:cc4e4527ea43:
/* mbed Microcontroller Library
* Copyright (c) 2019 ARM Limited
* SPDX-License-Identifier: Apache-2.0
*/
#include "mbed.h"
#include "platform/mbed_thread.h"
#include <stdio.h>
#include <stdlib.h>
/* マクロ定義、列挙型定義 */
#define LOW 0
// 使うロボットカーに合わせる
#define MBED01 1
#define MBED09 0.99
#define MBED MBED09 //使うロボットカーの番号にする
// 0: 前進 1: 旋回 2: 速度差 3: 障害物回避後戻る
float motorSpeedR1[9] = {0.5 , 0.5 , 0.5 , 0.5 };
float motorSpeedR2[9] = {0.5 , 0.5 , 0.5 , 0.5 };
float motorSpeedL1[9] = {0.5*MBED, 0.5*MBED, 0.5*MBED, 0.5*MBED};
float motorSpeedL2[9] = {0.5*MBED, 0.5*MBED, 0.5*MBED, 0.5*MBED};
/* trace用変数 */
int sensArray[32] = {0,4,2,4,1,4,2,4, // ライントレースセンサパターン
3,4,1,4,3,4,1,4, // 0:前回動作継続
5,1,5,1,5,1,5,1, // 1:高速前進
5,1,5,1,5,1,5,1}; // 2:左が速い前進
// 3:右が速い前進
// 4:右旋回
// 5:左旋回
/* 操作モード定義 */
enum MODE{
READY = -1, // -1：待ち
ADVANCE = 1, // 1：前進
RIGHT, // 2：右折
LEFT, // 3：左折
BACK, // 4：後退
STOP, // 5：停止
LINE_TRACE, // 6：ライントレース
AVOIDANCE, // 7：障害物回避
SPEED, // 8：スピード制御
LT_R, // 9:低速右折(ライントレース時)
LT_L, // 10:低速左折(ライントレース時)
AVOID_R, // 11:ライントレース＋障害物回避右
AVOID_L // 12:ライントレース＋障害物回避左
};
/* ピン配置 */
DigitalOut trig(p6); // 超音波センサtrigger
DigitalIn echo(p7); // 超音波センサecho
PwmOut servo(p25); // サーボ
// 2輪
DigitalIn ss1(p8); // ライントレースセンサ(左)
DigitalIn ss2(p9); // ライントレースセンサ
DigitalIn ss3(p10); // ライントレースセンサ
DigitalIn ss4(p11); // ライントレースセンサ
DigitalIn ss5(p12); // ライントレースセンサ(右)
// 2輪
PwmOut motorR2(p22); // 右モーター後退
PwmOut motorR1(p21); // 右モーター前進
PwmOut motorL2(p23); // 左モーター後退
PwmOut motorL1(p24); // 左モーター前進
DigitalOut led1(LED1); // ボードのLED1(右)
DigitalOut led4(LED4); // ボードのLED4(左)
int run = 0; // 走行モード
int speed = 0; // 走行スピード
int dist = 1000; // 障害物までの距離
int avoid_flag = 0; // 障害物検知したかどうかのフラグ
// 障害物回避用変数
Timer timer; // 距離計測用タイマ
Timer t; // 回避時間計測用タイマ
int DT = 10000; // 障害物までの距離
int flag_a = 0; // 障害物有無のフラグ
const int limit = 35; // 障害物の距離の限界値(単位：cm)
int t1 = 0; // 検知にかかった時間
LocalFileSystem local("local"); // ファイルを扱うときに使う
RawSerial pc(USBTX, USBRX); // PCとシリアル通信するときに使う
void motor(); // モーター制御関数
int watch(); // 障害物検知関数
void avoidance(); // 障害物回避関数
void motor(){
switch(run){
// 前進
case ADVANCE:
motorR1 = motorSpeedR1[speed]; // 右前進モーターON
motorR2 = LOW; // 右後退モーターOFF
motorL1 = motorSpeedL1[speed]; // 左前進モーターON
motorL2 = LOW; // 左後退モーターOFF
break;
// 右旋回
case RIGHT:
motorR1 = LOW; // 右前進モーターOFF
motorR2 = motorSpeedR2[speed]; // 右後退モーターON
motorL1 = motorSpeedL1[speed]; // 左前進モーターON
motorL2 = LOW; // 左後退モーターOFF
break;
// 左旋回
case LEFT:
motorR1 = motorSpeedR1[speed]; // 右前進モーターON
motorR2 = LOW; // 右後退モーターOFF
motorL1 = LOW; // 左前進モーターOFF
motorL2 = motorSpeedL2[speed]; // 左後退モーターON
break;
// 後退
case BACK:
motorR1 = LOW; // 右前進モーターOFF
motorR2 = motorSpeedR2[speed]; // 右後退モーターON
motorL1 = LOW; // 左前進モーターOFF
motorL2 = motorSpeedR2[speed]; // 左後退モーターON
break;
// 停止
case STOP:
motorR1 = LOW; // 右前進モーターOFF
motorR2 = LOW; // 右後退モーターOFF
motorL1 = LOW; // 左前進モーターOFF
motorL2 = LOW; // 左後退モーターOFF
break;
// 前進(左が速い)
case LT_R:
motorR1 = motorSpeedR2[speed]; // 右前進モーターON
motorR2 = LOW; // 右後退モーターOFF
motorL1 = motorSpeedL1[speed]; // 左前進モーターON
motorL2 = LOW; // 左後退モーターOFF
break;
// 前進(右が速い)
case LT_L:
motorR1 = motorSpeedR1[speed]; // 右前進モーターON
motorR2 = LOW; // 右後退モーターOFF
motorL1 = motorSpeedL2[speed]; // 左前進モーターON
motorL2 = LOW; // 左後退モーターOFF
break;
// ライントレース＋障害物回避右
case AVOID_R:
motorR1 = 0.5; // 右前進モーターON
motorR2 = LOW; // 右後退モーターOFF
motorL1 = 1; // 左前進モーターON
motorL2 = LOW; // 左後退モーターOFF
break;
// ライントレース＋障害物回避左
case AVOID_L:
motorR1 = 1; // 右前進モーターON
motorR2 = LOW; // 右後退モーターOFF
motorL1 = 0.5; // 左前進モーターON
motorL2 = LOW; // 左後退モーターOFF
break;
}
}
void trace(){
int sensor1 = ss1;
int sensor2 = ss2;
int sensor3 = ss3;
int sensor4 = ss4;
int sensor5 = ss5;
int sensD = 0;
/* センサー値の決定 */
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;
/* センサー値によって場合分け */
switch(sensArray[sensD]){
case 1:
run = ADVANCE; // 高速で前進
speed = 0;
break;
case 2:
run = LT_R; // 前進(左が速い)
speed = 2;
break;
case 3:
run = LT_L; // 前進(右が速い)
speed = 2;
break;
case 4:
run = RIGHT; // 右旋回
speed = 1;
break;
case 5:
run = LEFT; // 左旋回
speed = 1;
break;
default:
break; // 前回動作を継続
}
motor();
}
int watch(){
do{
do{
trig = 0;
ThisThread::sleep_for(1); // 5ms待つ
trig = 1;
ThisThread::sleep_for(1); // 15ms待つ
trig = 0;
timer.start();
t1=timer.read_ms();
while(echo.read() == 0 && t1<3){
t1=timer.read_ms();
}
timer.stop();
timer.reset();
}while(t1 >= 3);
timer.start(); // 距離計測タイマースタート
while(echo.read() == 1){
}
timer.stop(); // 距離計測タイマーストップ
DT = (int)(timer.read_us()*0.01657); // 距離計算
if(timer.read_ms() > 1000){
DT = 1000;
break;
}
}while(DT > 1000);
if(DT > 400){
DT = 400;
}
timer.reset();
return DT;
}
void avoidance(int c){
dist = watch();
if(dist <= limit){
if(c == 0){
// 左に回避
run = AVOID_L;
motor();
ThisThread::sleep_for(300);
speed = 3;
run = LT_R;
motor();
ThisThread::sleep_for(500);
}else{
// 右に回避
run = AVOID_R;
motor();
ThisThread::sleep_for(300);
speed = 3;
run = LT_L;
motor();
ThisThread::sleep_for(500);
}
avoid_flag = 1;
}
}
int main(){
// 初期設定
motorR1.period_us(40); // PWM周期設定
motorR2.period_us(40); // PWM周期設定
motorL1.period_us(40); // PWM周期設定
motorL2.period_us(40); // PWM周期設定
//servo.period_us(40); // PWM周期設定
avoid_flag = 0; // 障害物回避していない
// ファイルに回避方向を保存して制御
FILE *fp;
const char* fname = "/local/avo.txt"; // ファイル名
int c; // 読み込んだデータの格納
if(( fp = fopen(fname, "r") ) == NULL){
// pc.printf("can't open.\r\n");
fclose(fp);
exit(1);
}
c = fgetc(fp); // ファイルからデータ読み込み
c -= '0'; // 文字を数字に変換
fclose(fp);
// pc.printf("c = %d\r\n", c);
// pc.printf("c - '0' = %d\r\n", c - '0');
if(( fp = fopen(fname, "w") ) == NULL ){
fclose(fp);
exit(1);
}
if(c == 0){ // 読み込んだデータが 0 なら 1 を書き込み
fputc('1', fp);
led4 = 1;
// pc.printf("1\r\n");
}else{ // 読み込んだデータが 1 なら 0 を書き込み
fputc('0', fp);
led1 = 1;
// pc.printf("0\r\n");
}
fclose(fp);
while (true) {
trace();
if(avoid_flag == 0){ // まだ障害物を検知していなければ
avoidance(c);
}
}
}