Arduino小车自动避障

#include<Servo.h> //引用库
//因为很多子函数要用这个变量，所以把servo定义称全局变量，作用域是整个代码文件
Servo myServo;

//前进
void Forward(){
  digitalWrite(2,LOW);
  digitalWrite(3,HIGH);
  digitalWrite(4,HIGH);
  digitalWrite(5,LOW);
}
//后退
void BackOff(){
  digitalWrite(2,HIGH);
  digitalWrite(3,LOW);
  digitalWrite(4,LOW);
  digitalWrite(5,HIGH);
}
//左转
void TurnLeft(){
  //小车左转（左轮后退）
  digitalWrite(2,HIGH);
  digitalWrite(3,LOW);
  //小车左转（右轮前进）
  digitalWrite(4,HIGH);
  digitalWrite(5,LOW);
}
//右转
void TurnRight(){
  //小车右转（右轮后退）
  digitalWrite(4,LOW);
  digitalWrite(5,HIGH);
  //小车右转（左轮前进）
  digitalWrite(2,LOW);
  digitalWrite(3,HIGH);
}

//停止
void Stop(){
  digitalWrite(2,LOW);
  digitalWrite(3,LOW);
  digitalWrite(4,LOW);
  digitalWrite(5,LOW);
}

//获取距离
long getLen(){
  //发送一个10us的信号给超声波
  digitalWrite(9,LOW);
  delayMicroseconds(2);
  digitalWrite(9,HIGH);
  delayMicroseconds(10);
  digitalWrite(9,LOW);//超声波内部开始震荡，准备发送波
  long time;
  long length;
  //关注Echo高电平维持的时间，代表超声波发送到返回的时间
  time = pulseIn(8,HIGH);
  //距离= 时间(us) * 速度(340m/s)=》 微秒 * 厘米 / 往返 2
  // 微秒转秒；米转厘米；=》(time/1000000) * (340*100) / 2
  length = time * 0.017000;
  return length;
}

void Init(){
  //put your setup code here, to run once:
  //串口初始化
  //配置2，3口为输出引脚(左轮初始化)
  pinMode(2,OUTPUT);
  pinMode(3,OUTPUT);
  //配置4，5口为输出引脚(右轮初始化)
  pinMode(4,OUTPUT);
  pinMode(5,OUTPUT);
  //Trig接9，通过9发送一个触发信号给超声波
  pinMode(9,OUTPUT);
  //Echo接8，通过读取8高电平维持的实践，确认超声波哦在空气中传播的时间
  pinMode(8,INPUT);
  //pinMode(LED_BUILTIN,OUTPUT);
  //监听串口
  Serial.begin(9600);
  //把舵机黄色信号线插在ardino的引脚10
  myServo.attach(10);
}


void setup() {
  // put your setup code here, to run once:
  Init();
}

void loop() {
  // put your main code here, to run repeatedly:
  long centerLen;
  long leftLen;
  long rightLen;
  myServo.write(100);//居中
  delay(500);
  centerLen = getLen();
  Serial.print("中间的距离是：");
  Serial.println(centerLen);
  //检测到前方有障碍物
  if(centerLen < 50){
    Stop();
    //右边摇头确认距离
    myServo.write(30);//右边
    delay(500);
    rightLen = getLen();
    Serial.print("右边的距离是：");
    Serial.println(rightLen);

    //左边摇头确认距离
    myServo.write(170);//左边
    delay(500);
    leftLen = getLen();
    Serial.print("左边边的距离是：");
    Serial.println(leftLen);

    //回正
    myServo.write(100);//居中
    delay(500);
    //左边大于右边往左边走
    if(leftLen > rightLen){
      TurnLeft();
      Serial.println("左转");
      delay(100);
      Stop();
    }else{
      TurnRight();
      Serial.println("右转");
      delay(100);
      Stop();
    }

  }else{//前方无障碍物，前进
    Forward();
  }

}