基于Arduino的超声波智能避障小车

依赖库下载:
Servo

依赖库的安装方法，如不清楚，可以参考官方文档《安装其他的Arduino库》

所需器件：
Arduino开发板或Atmega328P芯片、霍尔传感器、超声波模块、舵机、电池

实物图：

程序如下：

#include <Servo.h> 
#include <stdlib.h>
#define speed1 5  //定义EA(PWM调速)接口
#define IN1 6  
#define IN2 7     
#define speed2 11  //定义EB(PWM调速)接口
#define IN3 12  
#define IN4 13    
#define sensor 4  //霍尔传感器
#define TrigPin 8  //超声波模块触发控制信号输入端口 
#define EchoPin 9  //超声波模块回响信号输入端口
Servo myservo;  //创建一个舵机控制对象  
float route = 0, distance, distance_right, distance_left; 
int Loop = 0, temploop = 0, rate = 0;
boolean sign = 0;
unsigned long seconds, temp_seconds = 1;
int Speed = 100;

void setup() {
  Serial.begin(9600);  //初始化串口通信及连接SR04的引脚
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(speed1, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(speed2, OUTPUT);
  pinMode(sensor, INPUT);
  pinMode(TrigPin, OUTPUT);  //输出超声波信号  
  pinMode(EchoPin, INPUT);  //要检测引脚上输入的脉冲宽度，需要先设置为输入状态
  myservo.attach(10);  //该舵机由arduino第10脚控制  
}

void Millis() {
  if (seconds > temp_seconds) {  //每过一秒      
    rate = 2 * temploop;
    route = 0.2 * Loop;

    temploop = 0;
    temp_seconds ++;

    Display();
  }
}

void count() {  //计算轮子转动的圈数
  if (digitalRead(sensor) == HIGH)
    sign = 1;
  if (digitalRead(sensor) == LOW && sign == 1) {
    Loop ++;
    temploop ++;
    sign = 0;
  }
}

void Display() {
  // Serial.print("Speed: ");
  // Serial.print(rate);
  // Serial.println(" dm/s");  

  // Serial.print("Loop: ");
  // Serial.println(Loop);

  // Serial.print("Distance: ");
  // Serial.print(route);
  // Serial.println(" m"); 

  // Serial.print("Distance_front: ");
  // Serial.print(distance);
  // Serial.println(" cm");

  // Serial.println();

  // Serial.print('"'); 
  // Serial.print("Speed");
  // Serial.print('"');
  // Serial.print(':');
  // Serial.print('"');
  // Serial.print(rate);
  // Serial.print('"');
  // Serial.print(",");  

  Serial.print(rate);
  Serial.print(" ");

  Serial.print(Loop);
  Serial.print(" ");

  Serial.print(route);
  Serial.print(" ");

  Serial.print(distance);
  Serial.print(" ");

  // Serial.println();
}

//参数pin是输入的高低电平的IO口，pwmpin表示输入的PWM波形的IO口，state指电机状态(正转或反转)，val是调速值大小0-255
void motor_right(int state, int val) {
  if (state == 1) {
    analogWrite(speed1, val);  //前进
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
  } else if (state == -1) {
    analogWrite(speed1, val);  //后退
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, HIGH);
  } else if (state == 0) {
    analogWrite(speed1, val);  //停止
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, LOW);
  }
}

void motor_left(int state, int val) {
  if (state == 1) {
    analogWrite(speed2, val);  //前进
    digitalWrite(IN3, HIGH);
    digitalWrite(IN4, LOW);
  } else if (state == -1) {
    analogWrite(speed2, val);  //后退
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, HIGH);
  } else if (state == 0) {
    analogWrite(speed2, val);  //停止
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, LOW);
  }
}

void forward() {
  motor_right(1, Speed);  //前进 
  motor_left(1, Speed);
}

void back() {
  motor_right(-1, Speed);  //后退
  motor_left(-1, Speed);
}

void left() {
  motor_left(-1, Speed);  //左转
  motor_right(1, Speed);   
}

void turn_left() {
  motor_left(-1, 150);  //左转
  motor_right(1, 150);  
  delay(600);
  forward();
}

void right() {
  motor_right(-1, Speed);  //右转
  motor_left(1, Speed);
}

void turn_right() {
  motor_right(-1, 150);   //右转
  motor_left(1, 150);
  delay(600);
  forward();
}

void stop() {
  if (State() == 'f') {
    back();
    delay(100);
    motor_right(0, 0);   //停止
    motor_left(0, 0);
  } else if (State() == 'b') {
    forward();
    delay(100);
    motor_right(0, 0);   //停止
    motor_left(0, 0);
  } else if(State() == 'l') {
    right();
    delay(100);
    motor_right(0, 0);   //停止
    motor_left(0, 0);
  } else if(State() == 'r') {
    left();
    delay(100);
    motor_right(0, 0);   //停止
    motor_left(0, 0);
  } else {
    motor_right(0, 0);   //停止
    motor_left(0, 0);
  }
}

String comdata = "";
boolean avoid = true;
void Bluetooth() {
  while (Serial.available() > 0) {
    comdata += char(Serial.read());
    delay(2);
  }

  if (comdata.length() > 0) {
    char ch = comdata[0];
    const char* c_s = comdata.c_str();
    int num = atoi(c_s);
    comdata = "";  
    
    if (num > 0 && num <= 255) {  //调速        
      Speed = num ;
      changeSpeed();
    } else if( ch == 'f' )  //前进   
      forward();    
    else if( ch == 'b' )  //后退
      back();
    else if( ch == 'l' )  //左转
      left();
    else if( ch == 'r' )  //右转
      right();
    else if( ch == 's' )  //停止
      stop();
    else if( ch == '+') {  //调速      
      Speed += 25;
      if(Speed == 225)
        Speed = 200;
      changeSpeed(); 
    } else if( ch == '-') {
      Speed -= 25;
      if(Speed == 25)
        Speed = 50;
      changeSpeed();
    } else if( ch == '0') {
      avoid = !avoid;
    }
  }      
}

void changeSpeed() {
  if (State() == 'f')
    forward();
  else if (State() == 'b')
    back();
  else if (State() == 'l')
    left();
  else if (State() == 'r')
    right();
}

void ultrasound() {
  digitalWrite(TrigPin, LOW);  // 产生一个10us的高脉冲去触发TrigPin
  delayMicroseconds(2); 
  digitalWrite(TrigPin, HIGH); 
  delayMicroseconds(10);
  digitalWrite(TrigPin, LOW);     
  distance = pulseIn(EchoPin, HIGH) / 58.00;  // 检测脉冲宽度，并计算出距离
  //Serial.print("distance: ");
  //Serial.print(distance); 
  //Serial.println(" cm"); 
  delay(10);
}

char State() {
  if (digitalRead(IN1) == HIGH &&  digitalRead(IN2) == LOW &&digitalRead(IN3) == HIGH &&  digitalRead(IN4) == LOW)
    return 'f';
  if (digitalRead(IN1) == LOW &&  digitalRead(IN2) == HIGH &&digitalRead(IN3) == LOW &&  digitalRead(IN4) == HIGH)
    return 'b';
  if (digitalRead(IN1) == HIGH &&  digitalRead(IN2) == LOW &&digitalRead(IN3) == LOW &&  digitalRead(IN4) == HIGH)
    return 'l';
  if (digitalRead(IN1) == LOW &&  digitalRead(IN2) == HIGH &&digitalRead(IN3) == HIGH &&  digitalRead(IN4) == LOW)
    return 'r';
}

void servo() {
  if (distance < 20 && State() == 'f') {
  	stop();  //小于20cm时先停止

    // Serial.print("front: ");  //前方距离
    // Serial.print(distance); 
    // Serial.println(" cm");     
     
    myservo.write(0);  //右边距离
    delay(400);
    ultrasound();
    distance_right = distance;
    // Serial.print("right: ");
    // Serial.print(distance_right); 
    // Serial.println(" cm"); 

    myservo.write(180);  //左边距离
    delay(600);
    ultrasound();
    distance_left = distance;
    // Serial.print("left: ");
    // Serial.print(distance_left); 
    // Serial.println(" cm");  
    // Serial.println();

    myservo.write(90);  //回到中间
    delay(400);  

    if (distance_left > distance_right)
    	turn_left();

    if (distance_left < distance_right)
    	turn_right(); 
  }
}

void loop() {
  seconds = millis() / 1000;
  ultrasound();
  if(avoid) 
    servo();
  count();
  Bluetooth();   
  Millis();
}