Arduino智能小车超声波测距避障基本思路总结

前几天已经实现了小车的循迹功能，下面是通过写避障的一些想法：

超声波原理：超声波传感器先发送一个信号，信号遇到障碍物后就会被反弹回来再被超声波传感器接收，接收到的高电平持续的时间就是超声波从发射到接收到的时间，所以测试的距离=（高电平时间*声速）/2；

再将距离单位换算成厘米所用函数：distance=pulseIn（接收信号的引脚echo,HIGH）/58; （声速为340m/s）；

测距避障基本思路：

1.先让小车行走的同时检查正前方有无障碍，如果没有就继续向前走，有的话就测距并判断距离是否达到避障的条件（避障距离可视实际情况而定）；

2.需要转弯避障时，总不能继续往前走吧，所接下来就先让小车停下，这时就该检查一下左右两边的情况，所以需要在超声波传感器下加一个舵机让超声波随着舵机旋转左右摆动检查左右距障碍距离，那边距离小就转向哪边

下面是代码：

#include <Servo.h>
#include <FlexiTimer2.h>
Servo myservo;

#define STOP      0
#define FORWARD   1
#define BACKWARD  2
#define TURNLEFT  3
#define TURNRIGHT 4
int value=150;
int leftmotor1=4;
int leftmotor2=5;
int rightmotor1=6;
int rightmotor2=7;
int lefttrack=8;
int righttrack=9;
int echo=10;
int trig=11;
void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  pinMode(leftmotor1,OUTPUT);
  pinMode(leftmotor2,OUTPUT);                                
  pinMode(rightmotor1,OUTPUT);
  pinMode(rightmotor2,OUTPUT);
  pinMode(lefttrack,INPUT);
  pinMode(righttrack,INPUT);
  pinMode(trig,OUTPUT);
  pinMode(echo,INPUT);
  myservo.attach(12);
}

void loop() {
  // put your main code here, to run repeatedly:

 
    avoid();

}
void motor(int cmd)   //value表示速度
{
  switch(cmd){
    case FORWARD:
      analogWrite(leftmotor1, value);
      analogWrite(leftmotor2,0);
      analogWrite(rightmotor1,value);
      analogWrite(rightmotor2,0);
      break;
     case BACKWARD:
      analogWrite(leftmotor1, 0);
      analogWrite(leftmotor2,value);
      analogWrite(rightmotor1, 0);
      analogWrite(rightmotor2,value);
      break;
     case TURNLEFT:
      analogWrite(leftmotor1, 0);
      analogWrite(leftmotor2,value);
      analogWrite(rightmotor1,value);
      analogWrite(rightmotor2,0);
      break;
     case TURNRIGHT:
      analogWrite(leftmotor1, value);
      analogWrite(leftmotor2,0);
      analogWrite(rightmotor1,0);
      analogWrite(rightmotor2,value);
      break;
     default:
      analogWrite(leftmotor1,0);
      analogWrite(leftmotor2,0);
      analogWrite(rightmotor1,0);
      analogWrite(rightmotor2,0);
      break;
  }
}

void avoid()
{
  int dis[3]={0};
  int pos=90;
  motor(FORWARD);
  dis[1]=getdis();//前方距障碍距离
  if(dis[1]<30)
  {
    motor(STOP);
    for(pos=90;pos<=150;pos++)
    {
      myservo.write(pos);
      delay(15);
      }
      dis[0]=getdis();
      for(int i=0;i<3;i++)
      {
        myservo.write(150);
        delay(300);
        int val=getdis();
        dis[0]=min(dis[0],val);//左边距障碍距离测三次取最小值
        }
      for(pos=150;pos>=30;pos--)
      {
        myservo.write(pos);
        delay(15);
        if(pos==90)
        {
          dis[1]=getdis();
          }
        }
        dis[2]=getdis();
      for(int i=0;i<3;i++)
      {
        myservo.write(30);
        delay(300);
        int val=getdis();
        dis[2]=min(dis[2],val);//右边距障碍距离测三次取最小值
        }
        if(dis[0]>=dis[2])
        {
          motor(TURNLEFT);
          }
          else
          {
            motor(TURNRIGHT);
            }
     for(pos=30;pos<=90;pos++)
        {
          myservo.write(pos);
          delay(15);
          }
  }
  }
  int getdis()
  {
    digitalWrite(trig,LOW);
    delayMicroseconds(2);//缓冲
    digitalWrite(trig,HIGH);
    delayMicroseconds(10);
    digitalWrite(trig,LOW);
    int distance=pulseIn(echo,HIGH);
    distance=distance/58;//换算成厘米
    if(distance>=50)
    return 50;
    else
    return distance;
    }