Arduino小车感应跟随

//前进
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);
}

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

void loop() {
  // put your main code here, to run repeatedly:
  delay(20); //延迟一下，放缓超声波交互速度，比较不卡顿（加强体验）
  long len = getLen();
  //距离大于3且小于13，后退
  if(len > 3 && len < 13){
    BackOff();
  }
  //距离大于14且小于40，前进
  if(len > 14 && len < 40){
    Forward();
  }
  //距离大于50,停下来
  if(len > 50){
    Stop();
  }
  //Serial.println(len);
  //Serial.println("cm");
}