arduino小车（二）：超声波避障_超声波避障小车代码

超声波避障在小车的三个功能（循迹、避障、蓝牙控制）中耗费的时间最长，当我们把敲好的代码下载到arduino板上测试时，小车的行动总会与代码的逻辑不符。

我们的逻辑是：先让小车走直线，同时HC-SR04开始工作不断检测小车与前方障碍物的距离。当小车与障碍物的距离小于50cm时小车停止前进，小车停止后舵机带动HC-SR04先向左转，如果左侧没有障碍物则小车原地左转，否则HC-SR04的探测头转向小车的右侧，如果右侧没有障碍物则小车向右转，否则小车掉头。
在实地测试时，小车停止运动后探测头先向左转，如果左侧有障碍物小车会直接掉头，跳过探头向右转并测距的步骤。
以下为小车代码：

#include<Servo.h>
int inputPin = 8; // 定义超声波信号接收接口
int outputPin = 9; // 定义超声波信号发出接口

int STBY = 11; //控制电机驱动状态引脚

int PWMA = 3; //A电机速度控制
int AIN1 = 23; //A电机方向
int AIN2 = 22; 

int PWMB = 5; //与上述A做对应即可
int BIN1 = 25; 
int BIN2 = 24; 

Servo myservo;//定义舵机变量名
void setup()
{
  Serial.begin(9600);//设置串口波特率为9600
  myservo.attach(10);//定义舵机接口10 
  pinMode(inputPin, INPUT);
  pinMode(outputPin, OUTPUT);
  pinMode(STBY, OUTPUT);
  pinMode(PWMA, OUTPUT);
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(PWMB, OUTPUT);
  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);
  myservo.write(90);  //初始化舵机方向
}

void loop()
{
  myservo.write(90);
  int distance=MTD();    //超声波测距
  if(distance<50)  //前方50cm处有障碍
  {
    move(1,0,1);      //小车停下
    move(2,0,1);
    delay(1000);
    myservo.write(0);  //舵机左转
    delay(500);
    int qdistance=MTD();   //测距
    if(qdistance<50)  // 左边50cm处有障碍
    {
      move(1,0,1);
      move(2,0,1);
      delay(1000);
   	  myservo.write(180);    //舵机右转
      delay(500);
   	  int qqdistance=MTD();     //测距
   	  if(qqdistance<20) //右边50cm处有障碍
      {
        move(1,0,1);
        move(2,0,1);
        move(1,128,1);      //左电机向前，右电机向后，以实现掉头这一过程
        move(2,128,0);
        delay(2000);        //延迟时间较长，以实现成功掉头
        move(1,0,1);
        move(2,0,1);
      }
      else     //右边50cm无障碍
      {
        delay(1000);
        move(1,100,1);    //左电机向前，右电机向后，以实现右转
        move(2,100,0);    
        delay(1000);       //延迟时间短，以实现右转
        move(1,0,1);
        move(2,0,1);
      }
    }
    else        //左边50cm处无障碍
    {
      delay(1000);         
      move(1,100,0);       //左电机向后，右电机向前，以实现左转
      move(2,100,1);
      delay(1000);
      move(1,0,1);
      move(2,0,1);
    }
  }
  else     //前方50cm处无障碍
  {
    move(1,128,1);
    move(2,128,1);
  } 
  delay(500);//刷新时间500ms，也就是0.5s
}
int MTD(){                     
  digitalWrite(outputPin, LOW); // 使发出发出超声波信号接口低电平2ms，以得到干净的高电平。
  delayMicroseconds(2);
  digitalWrite(outputPin, HIGH); // 使发出发出超声波信号接口高电平10ms，这里是至少10μs
  delayMicroseconds(10);
  digitalWrite(outputPin, LOW); // 保持发出超声波信号接口低电平
  int distance = pulseIn(inputPin, HIGH); // 读出脉冲时间
  distance = distance / 58; // 将脉冲时间转化为距离（单位：厘米）
  return distance;
}

void move(int motor, int speed, int direction){  
  digitalWrite(STBY, HIGH); //disable standby
  
  boolean inPin1 = HIGH;

  boolean inPin2 = LOW;

  if(direction == 1){
    inPin1 = LOW;
    inPin2 = HIGH;
  }

  if(motor == 1){
    digitalWrite(AIN1, inPin1);
    digitalWrite(AIN2, inPin2);
    analogWrite(PWMA, speed);
  }
  else{
    digitalWrite(BIN1, inPin1);
    digitalWrite(BIN2, inPin2);
    analogWrite(PWMB, speed);
  }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122

小车对障碍物的检测是一个时间极短的过程。所以在小车停止时先要给舵机足够的时间让传感器探头转动到指定位置之后再检测前方是否有障碍物。