Arduino用モータードライブシールド「Arudumoto」でモーターを回す（5）

フロントセンサーを取り付け、障害物を検知したら左に避ける

前回までのクローラーは前進・後退、右回り・左曲り、右旋回・左旋回のみで、万が一障害物があった場合には避けることができませんでした。
そこで、クローラーの前部に距離計を付けて障害物を検知し、検知された時には左に曲がることで障害物を避けてみました。

             

使用した距離計はシャープ製の「GP2Y0A21YK0F」で、灯光部から赤外線を投射して測定対象物から跳ね返ってくる赤外線の角度を測定し、電圧に換算し出力するものです。
そのため、対象物の反射率の影響は少なくなります。

距離計出力グラフ

距離と電圧出力はグラフのようになり、非直線の特性を持っており、何らかの換算が必要となりますが、すでに換算式が発表されておりますのでそれを利用することとします。

換算式　R=(6787/(V-3))/3

距離 5cm 以下で急激に出力電圧低下しているので、使用範囲は10cm～80cmになります。

配線図

距離計の出力をアナログ０ピンに接続し、障害物までの距離が２０ｃｍ以下になったらクローラーを停止した後、左に方向を変えて再度前進を始めます。
さらに、障害物を検知した時にＬＥＤランプが点灯するようにしております。

クローラー

距離測定ルーチン
 今回のスケッチは「Arduinoで電子工作をはじめよう！」第２版を参考にさせて頂きました。

//フロントセンサー
int front()
 {
  int tmp, distance;

  tmp = analogRead(FRONT_SEN);
  if(tmp < 4 ) tmp=4;
  distance = (6787 / (tmp-3))-4;
  return(distance);
 }

トータルスケッチ

 

//モーターピン設定
 int pwm_a = 3;
 int dir_a = 12;
 int pwm_b = 11;
 int dir_b = 13;

//スタートスイッチ　LEDピン設定
 int ST_SWITCH = 7;
 int LED = 6;
 int SEN_LED =5;

//フロントセンサー設定
 int FRONT_SEN = 0;

//サブルーチン

//前進設定
void forw()
 {
 digitalWrite(dir_a,HIGH);
 digitalWrite(dir_b,HIGH);
 }

//後退設定
void back()
 {
 digitalWrite(dir_a,LOW);
 digitalWrite(dir_b,LOW);
 } 

//前進
void forward(int mspeed,int mtime)
 {
 digitalWrite(dir_a,HIGH);
 digitalWrite(dir_b,HIGH);
 analogWrite(pwm_a,mspeed);
 analogWrite(pwm_b,mspeed);
 if(mtime > 0){
  delay(mtime);
  analogWrite(pwm_a,0);
  analogWrite(pwm_b,0);
  }
 }

//後退
void backward(int mspeed,int mtime)
 {
 digitalWrite(dir_a,LOW);
 digitalWrite(dir_b,LOW);
 analogWrite(pwm_a,mspeed);
 analogWrite(pwm_b,mspeed);
 if(mtime > 0){
  delay(mtime);
  analogWrite(pwm_a,0);
  analogWrite(pwm_b,0);
  }
 }
//９０度左曲り
void left(int mspeed,int mtime)
 {
 digitalWrite(dir_b,LOW);
 digitalWrite(dir_a,HIGH);
 analogWrite(pwm_b,mspeed);
 analogWrite(pwm_a,mspeed);
 if(mtime>0){
  delay(mtime);
  analogWrite(pwm_b,0);
  analogWrite(pwm_a,0);
  }
 }
//９０度右曲り
void right(int mspeed,int mtime)
 {
 digitalWrite(dir_b,HIGH);
 digitalWrite(dir_a,LOW);
 analogWrite(pwm_b,mspeed);
 analogWrite(pwm_a,mspeed);
 if(mtime>0){
  delay(mtime);
  analogWrite(pwm_b,0);
  analogWrite(pwm_a,0);
  }
 }

//前進左旋回
void FLcircle(int amspeed,int bmspeed,int mtime)
 {
 digitalWrite(dir_a,HIGH);
 digitalWrite(dir_b,HIGH);
 analogWrite(pwm_a,amspeed);
 analogWrite(pwm_b,bmspeed);
 if(mtime > 0){
  delay(mtime);
  analogWrite(pwm_a,0);
  analogWrite(pwm_b,0);
  }
 }

//前進右旋回
void FRcircle(int amspeed,int bmspeed,int mtime)
 {
 digitalWrite(dir_a,HIGH);
 digitalWrite(dir_b,HIGH);
 analogWrite(pwm_a,amspeed);
 analogWrite(pwm_b,bmspeed);
 if(mtime > 0){
  delay(mtime);
  analogWrite(pwm_a,0);
  analogWrite(pwm_b,0);
  }
 }

//停止
void stopped()
 {
 digitalWrite(dir_a,LOW);
 digitalWrite(dir_b,LOW);
 analogWrite(pwm_a,0);
 analogWrite(pwm_b,0);
 }

//緩やかにスタート
void fadein()
 {
  for(int i=0;i<=200;i+=1){
  analogWrite(pwm_a,i);
  analogWrite(pwm_b,i);
  delay(30);
  }
 }

//緩やかに停止
void fadeout()
 {
  for(int i=200;i>=0;i-=1){
  analogWrite(pwm_a,i);
  analogWrite(pwm_b,i);
  delay(30);
  }
 }

//フロントセンサー
int front()
 {
  int tmp, distance;

  tmp = analogRead(FRONT_SEN);
  if(tmp < 4 ) tmp=4;
  distance = (6787 / (tmp-3))-4;
  return(distance);
 }

 
void setup()
  {
  //ピンモード設定
   pinMode(pwm_a, OUTPUT);
    pinMode(dir_a, OUTPUT);
 
   pinMode(pwm_b, OUTPUT);
    pinMode(dir_b, OUTPUT); 
 
  pinMode(ST_SWITCH,INPUT);
  pinMode(LED,OUTPUT);
  pinMode(SEN_LED,OUTPUT);
   }

void loop()
   {

   while(1){
 if(digitalRead(ST_SWITCH)==LOW)break;
 }

  digitalWrite(LED,HIGH);

   delay(2000);

while(1){
 

 forward(100,0);

 if( front() < 20){
  digitalWrite(SEN_LED,HIGH);
  
  stopped();
  delay(500);

  backward(100,1000);
  left(100,1000);

  digitalWrite(SEN_LED,LOW);
  }
 }

stopped();
delay(500);
digitalWrite(LED,LOW);

 }