arduinoによる衝突回避型ロボットカー
距離センサーであるHCSR40を使ってみたくて衝突回避型ロボットカーを作ってみた。
- 車体は大分前に買ったラインとレースする車のものを流用して、東芝のTA7291２個でモーターをドライブ
- 電池は単三四本（実は空気電池四本で合計5.2V
- ＣＰＵはarduino NANO
衝突防止のアルゴリズムはネットで検索して適当に流用した。また、HCSR40の取り付け位置が若干高いのか、低い位置にある障害物を検知出来ないので別途近接センサーを前部両側に取り付けた。
しかし、衝突回避部分のプログラムが複雑で十分理解していない部分もあるので動きは少し変である。
何が原因なのか解析するためにランプでもつけてどう言う理由でその動作が起こっているのか解析することとする。
プログラム
#include "configuration-Ver2.h"
#include //I2C Arduino Library
#define address 0x1E //0011110b, I2C 7bit address of HMC5883
Servo head;
/*motor control*/
void go_Advance(int move_time) //Forward
{
digitalWrite(dir1PinL, HIGH);
digitalWrite(dir2PinL,LOW);
digitalWrite(dir1PinR,HIGH);
digitalWrite(dir2PinR,LOW);
delay(move_time);
}
void go_Right(int move_time ) //Turn left
{
digitalWrite(dir1PinL, HIGH);
digitalWrite(dir2PinL,LOW);
digitalWrite(dir1PinR,LOW);
digitalWrite(dir2PinR,HIGH);
delay(move_time);
}
void go_Left(int move_time) //Turn right
{
digitalWrite(dir1PinL, LOW);
digitalWrite(dir2PinL,HIGH);
digitalWrite(dir1PinR,HIGH);
digitalWrite(dir2PinR,LOW);
delay(move_time);
}
void go_Back( int move_time ) //Reverse
{
digitalWrite(dir1PinL, LOW);
digitalWrite(dir2PinL,HIGH);
digitalWrite(dir1PinR,LOW);
digitalWrite(dir2PinR,HIGH);
delay(move_time);
}
void stop_Stop(int stop_time) //Stop
{
digitalWrite(dir1PinL, LOW);
digitalWrite(dir2PinL,LOW);
digitalWrite(dir1PinR,LOW);
digitalWrite(dir2PinR,LOW);
delay( stop_time );
}
/*set motor speed */
void set_Motorspeed()
{
int Speed;
int potention = analogRead(SpeedSet_PIN);
Speed = ((potention * 10)/1024) * 25.5;
Serial.println(Speed);
Serial.println(potention);
analogWrite(speedPin,Speed);
//analogWrite(speedPinR,speed_R);
}
//void buzz_ON() //open buzzer
//{
// digitalWrite(BUZZ_PIN, LOW);
//}
//void buzz_OFF() //close buzzer
//{
// digitalWrite(BUZZ_PIN, HIGH);
//}
//void alarm(){
// buzz_ON();
// delay(100);
// buzz_OFF();
//}
/*detection of ultrasonic distance*/
int watch(){
long howfar;
digitalWrite(Trig_PIN,LOW);
delayMicroseconds(5);
digitalWrite(Trig_PIN,HIGH);
delayMicroseconds(15);
digitalWrite(Trig_PIN,LOW);
howfar=pulseIn(Echo_PIN,HIGH);
howfar=howfar*0.01657; //how far away is the object in cm
Serial.println(howfar);
return round(howfar);
}
//
// 障害物回避処理
// 入力条件は障害物までの距離
// 結果：OK/NG(障害物を回避できた場合は『OK』)
//
int mv_avoid( int d ) {
int l, r , resp;
//digitalWrite( LED, HIGH );
resp = NG;
head.write(130); // 左を確認
delay(100);
l = watch();
head.write(45); // 右を確認
delay(100);
r = watch();
if (( l > d ) || ( l >= r )) { // 左方に障害物が無ければ左回転
head.write(135);
go_Left( T_LEFT );
//digitalWrite( LED, LOW );
resp = OK;
} else if (( r > d ) || ( r >= l )) { // 右方に障害物が無ければ右回転
head.write(45);
go_Right( T_RIGHT );
//digitalWrite( LED, LOW );
resp = OK;
}
//else if (( r > d ) || ( l >= r )) { // 左方に障害物が無ければ左回転
//head.write(135);
//go_Left( T_LEFT );
//digitalWrite( LED, LOW );
//resp = OK;
//} else if (( l > d ) || ( r >= l )) { // 右方に障害物が無ければ右回転
//head.write(45);
//go_Right( T_RIGHT );
//digitalWrite( LED, LOW );
//resp = OK;
// }
return resp;
}
void setup() {
/*setup TA7291P pin mode*/
pinMode(dir1PinL, OUTPUT);
pinMode(dir2PinL, OUTPUT);
pinMode(speedPin, OUTPUT);
pinMode(dir1PinR, OUTPUT);
pinMode(dir2PinR, OUTPUT);
// pinMode(speedPinR, OUTPUT);
stop_Stop(T_STOP);//stop move
/*init HC-SR04*/
pinMode(Trig_PIN, OUTPUT);
pinMode(Echo_PIN,INPUT);
/*init buzzer*/
// pinMode(BUZZ_PIN, OUTPUT);
// digitalWrite(BUZZ_PIN, HIGH);
// buzz_OFF();
digitalWrite(Trig_PIN,LOW);
/*init servo*/
head.attach(SERVO_PIN);
head.write(90);
/*baud rate*/
Serial.begin(115200);
//Initialize Serial and I2C communications
Wire.begin();
//Put the HMC5883 IC into the correct operating mode
Wire.beginTransmission(address); //open communication with HMC5883
Wire.write(0x02); //select mode register
Wire.write(0x00); //continuous measurement mode
Wire.endTransmission();
}
void loop() {
int d;
head.write(90); // 前を向く
delay(100); // ヘッドが動作完了するのを待つ
set_Motorspeed();
d = watch(); // 前方を確認
if ( d > BARRIER ) { // 前方に障害物が無ければ前進
//digitalWrite( LED, LOW );
Near_Check();
go_Advance(T_MOVE);
} else if ( d < BARRIER ) { // 障害物発見
//digitalWrite( LED, HIGH ); // 障害物まで至近距離
stop_Stop( T_STOP );
go_Back( T_BACK); // とりあえずバックして避ける
mv_avoid( BARRIER ); // 回避処理
// 障害物は見つけたが、未だ距離はあるので左右へ回避を試みる
}
//if ( mv_avoid( BARRIER ) == NG) {
// 回避できなかった。。。3方向全部障害(袋小路)
// stop_Stop( T_STOP );
// go_Back(T_BACK); // バックしてリトライ
// mv_avoid( BARRIER ); // 再度回避処理
// head.write(90); // ヘッドは前に向けておく
delay(100);
//}
//go_Advance(T_MOVE);
//stop_Stop( 1000);
//go_Left( T_LEFT );
// stop_Stop( 1000 );
// go_Right( T_RIGHT );
//stop_Stop( 1000);
//go_Back(T_MOVE);
// stop_Stop( 3000 );
}
void Near_Check() {
int valL = analogRead(NearL_PIN);
int valR = analogRead(NearL_PIN);
//Serial.print("L");
//Serial.println(valL);
if ((valL < 950) || (valR < 950)) {
stop_Stop(100);
go_Back(300);
if (valL <= valR) {
go_Right(300);} else {go_Left(300);};
}
------------------------------------------------------------------------------------
定義ファイル configuration-Ver2.h
/*Declare L298N Dual H-Bridge Motor Controller directly since there is not a library to load.*/
//Define L298N Dual H-Bridge Motor Controller Pins
#define dir1PinL 12 //Motor direction
#define dir2PinL 11 //Motor direction
//#define speedPinL 6 // Needs to be a PWM pin to be able to control motor speed
#define dir1PinR 10 //Motor direction
#define dir2PinR 9 //Motor direction
#define speedPin 5 // Needs to be a PWM pin to be able to control motor speed
#define SERVO_PIN 3 //servo connect to D9
#define Echo_PIN 7 // Ultrasonic Echo pin connect to D11
#define Trig_PIN 8 // Ultrasonic Trig pin connect to D12
#define NearL_PIN 0 // Analog PIN
#define NearR_PIN 1 // Analog PIN
#define SpeedSet_PIN 2 //Speed define PIN
//#define BUZZ_PIN 14
#define SPEED 150 //both sides of the motor speed
int leftscanval, centerscanval, rightscanval, ldiagonalscanval, rdiagonalscanval;
const int distancelimit = 30; //distance limit for obstacles in front
const int sidedistancelimit = 30; //minimum distance in cm to obstacles at both sides (the car will allow a shorter distance sideways)
int distance;
int numcycles = 0;
const int turntime = 800; //Time the robot spends turning (miliseconds)
int thereis;
#define BARRIER 30 // 障害物までの距離
#define BARRIER1 10 // 障害物までの距離(至近距離)
// 移動時間の定義
#define T_MOVE 500
//#define T_MOVE 1000
#define T_BACK 300
#define T_LEFT 300 // 左へ90度回転
#define T_RIGHT 300 // 右へ90度回転
#define DELAY 10
#define T_STOP 300
#define OK 0
#define NG 1
-------------------------------------------------------------------------------------
衝突回避部分の動きが今一なのでプログラムをみなおしてみた。バグもあったり停止時間を少し多めにしたりして動きは多少良くなった様である。
改善したプログラム
--------------------------------------------------------------------------------------
#include
#include "configuration-Ver2.h"
#include //I2C Arduino Library
#define address 0x1E //0011110b, I2C 7bit address of HMC5883
Servo head;
/*motor control*/
void go_Advance(int move_time) //Forward
{
digitalWrite(dir1PinL, HIGH);
digitalWrite(dir2PinL,LOW);
digitalWrite(dir1PinR,HIGH);
digitalWrite(dir2PinR,LOW);
delay(move_time);
}
void go_Right(int move_time ) //Turn left
{
digitalWrite(dir1PinL, HIGH);
digitalWrite(dir2PinL,LOW);
digitalWrite(dir1PinR,LOW);
digitalWrite(dir2PinR,HIGH);
delay(move_time);
}
void go_Left(int move_time) //Turn right
{
digitalWrite(dir1PinL, LOW);
digitalWrite(dir2PinL,HIGH);
digitalWrite(dir1PinR,HIGH);
digitalWrite(dir2PinR,LOW);
delay(move_time);
}
void go_Back( int move_time ) //Reverse
{
digitalWrite(dir1PinL, LOW);
digitalWrite(dir2PinL,HIGH);
digitalWrite(dir1PinR,LOW);
digitalWrite(dir2PinR,HIGH);
delay(move_time);
}
void stop_Stop(int stop_time) //Stop
{
digitalWrite(dir1PinL, LOW);
digitalWrite(dir2PinL,LOW);
digitalWrite(dir1PinR,LOW);
digitalWrite(dir2PinR,LOW);
delay( stop_time );
}
/*set motor speed */
void set_Motorspeed()
{
int Speed;
int potention = analogRead(SpeedSet_PIN);
Speed = ((potention * 10)/1024) * 25.5;
// Serial.println(Speed);
// Serial.println(potention);
analogWrite(speedPin,Speed);
//analogWrite(speedPinR,speed_R);
}
//void buzz_ON() //open buzzer
//{
// digitalWrite(BUZZ_PIN, LOW);
//}
//void buzz_OFF() //close buzzer
//{
// digitalWrite(BUZZ_PIN, HIGH);
//}
//void alarm(){
// buzz_ON();
// delay(100);
// buzz_OFF();
//}
/*detection of ultrasonic distance*/
int watch(){
long howfar;
digitalWrite(Trig_PIN,LOW);
delayMicroseconds(5);
digitalWrite(Trig_PIN,HIGH);
delayMicroseconds(15);
digitalWrite(Trig_PIN,LOW);
howfar=pulseIn(Echo_PIN,HIGH);
howfar=howfar*0.01657; //how far away is the object in cm
Serial.println(howfar);
return round(howfar);
}
//
// 障害物回避処理
// 入力条件は障害物までの距離
// 結果：OK/NG(障害物を回避できた場合は『OK』)
//
int mv_avoid( int d ) {
int l, r , resp;
//digitalWrite( LED, HIGH );
resp = NG;
head.write(140); // 左を確認
delay(400);
l = watch();
head.write(40); // 右を確認
delay(400);
r = watch();
if (( l > d ) || ( l >= r )) { // 左方に障害物が無ければ左回転
head.write(140);
go_Left( T_LEFT );
//digitalWrite( LED, LOW );
resp = OK;
} else if (( r > d ) || ( r >= l )) { // 右方に障害物が無ければ右回転
head.write(40);
go_Right( T_RIGHT );
//digitalWrite( LED, LOW );
resp = OK;
}
//else if (( r > d ) || ( l >= r )) { // 左方に障害物が無ければ左回転
//head.write(135);
//go_Left( T_LEFT );
//digitalWrite( LED, LOW );
//resp = OK;
//} else if (( l > d ) || ( r >= l )) { // 右方に障害物が無ければ右回転
//head.write(45);
//go_Right( T_RIGHT );
//digitalWrite( LED, LOW );
//resp = OK;
// }
return resp;
}
void setup() {
/*setup TA7291P pin mode*/
pinMode(dir1PinL, OUTPUT);
pinMode(dir2PinL, OUTPUT);
pinMode(speedPin, OUTPUT);
pinMode(dir1PinR, OUTPUT);
pinMode(dir2PinR, OUTPUT);
// pinMode(speedPinR, OUTPUT);
stop_Stop(T_STOP);//stop move
/*init HC-SR04*/
pinMode(Trig_PIN, OUTPUT);
pinMode(Echo_PIN,INPUT);
pinMode(13,OUTPUT); //プログラム監視
pinMode(4,OUTPUT); //プログラム監視
/*init buzzer*/
// pinMode(BUZZ_PIN, OUTPUT);
// digitalWrite(BUZZ_PIN, HIGH);
// buzz_OFF();
digitalWrite(Trig_PIN,LOW);
/*init servo*/
head.attach(SERVO_PIN);
head.write(90);
/*baud rate*/
Serial.begin(115200);
//Initialize Serial and I2C communications
Wire.begin();
//Put the HMC5883 IC into the correct operating mode
Wire.beginTransmission(address); //open communication with HMC5883
Wire.write(0x02); //select mode register
Wire.write(0x00); //continuous measurement mode
Wire.endTransmission();
}
void loop() {
int d;
head.write(90); // 前を向く
delay(100); // ヘッドが動作完了するのを待つ
set_Motorspeed();
d = watch(); // 前方を確認
if ( d > BARRIER ) { // 前方に障害物が無ければ前進
//digitalWrite( LED, LOW );
Near_Check();
go_Advance(T_MOVE);
} else if ( d < BARRIER ) { // 障害物発見
//digitalWrite( LED, HIGH ); // 障害物まで至近距離
stop_Stop( T_STOP );
digitalWrite( 13, HIGH );
go_Back( T_BACK); // とりあえずバックして避ける
digitalWrite(13,LOW);
stop_Stop( T_STOP );
mv_avoid( BARRIER ); // 回避処理
// 障害物は見つけたが、未だ距離はあるので左右へ回避を試みる
}
//if ( mv_avoid( BARRIER ) == NG) {
// 回避できなかった。。。3方向全部障害(袋小路)
// stop_Stop( T_STOP );
// go_Back(T_BACK); // バックしてリトライ
// mv_avoid( BARRIER ); // 再度回避処理
// head.write(90); // ヘッドは前に向けておく
delay(100);
//}
//go_Advance(T_MOVE);
//stop_Stop( 1000);
//go_Left( T_LEFT );
// stop_Stop( 1000 );
// go_Right( T_RIGHT );
//stop_Stop( 1000);
//go_Back(T_MOVE);
// stop_Stop( 3000 );
}
void Near_Check() {
int valL = analogRead(NearL_PIN);
int valR = analogRead(NearR_PIN);
Serial.print("L");
Serial.println(valL);
if ((valL < 950) || (valR < 950)) {
stop_Stop(T_STOP);
digitalWrite(4,HIGH);
go_Back(1000);
digitalWrite(4,LOW);
stop_Stop(T_STOP);
if (valL <= valR) {
go_Right(500);} else {go_Left(500);};
}
// Serial.print("R");
// Serial.println(valR);
// if (valR < 950) {
// stop_Stop(100);
// go_Back(200);
// go_Left(500);
// }
}
/*
-------------------------------------------------------------------------------------
定義ファイル configuration-Ver2.h
/*Declare L298N Dual H-Bridge Motor Controller directly since there is not a library to load.*/
//Define L298N Dual H-Bridge Motor Controller Pins
#define dir1PinL 12 //Motor direction
#define dir2PinL 11 //Motor direction
//#define speedPinL 6 // Needs to be a PWM pin to be able to control motor speed
#define dir1PinR 10 //Motor direction
#define dir2PinR 9 //Motor direction
#define dirRIFor 2 //remocon number 2
#define dirRILeft 3 //remocon number 4
#define dirRIRight 4 //remocon number 6
#define dirRIBack 5 //remocon number 8
#define SPEED 150 //both sides of the motor speed
int leftscanval, centerscanval, rightscanval, ldiagonalscanval, rdiagonalscanval;
const int distancelimit = 30; //distance limit for obstacles in front
const int sidedistancelimit = 30; //minimum distance in cm to obstacles at both sides (the car will allow a shorter distance sideways)
int distance;
int numcycles = 0;
const int turntime = 800; //Time the robot spends turning (miliseconds)
int thereis;
#define BARRIER 30 // 障害物までの距離
#define BARRIER1 10 // 障害物までの距離(至近距離)
// 移動時間の定義
#define T_MOVE 500
//#define T_MOVE 1000
#define T_BACK 300
#define T_LEFT 300 // 左へ90度回転
#define T_RIGHT 300 // 右へ90度回転
#define DELAY 10
#define T_STOP 300
#define OK 0
#define NG 1