写一个arduino程序，控制车(四个电机驱动轮）实现循迹避障功能，要求如下： 1.两个红外传感器（TCRT5000）辅助车实现沿着单一黑色线行驶； 2.超声波传感器（HC-SR04）判断前方是否有障碍，如果有障碍，车子右转优先避障，绕过障碍物之后再次回到黑色线轨迹上（红外传感器辅助判断）。 3.配备光敏传感器控制LED灯，外界环境亮度不够时点亮LED。 备注：1.左边两个轮子同步运转，右边两个轮子运动同步，利用左右的差速实现车身偏转，四个轮子由电机驱动模块L9110驱动。

#include <AFMotor.h>
const int speed = 100; //设置轮轴速度
AF_DCMotor motor1(1); //左前电机1
AF_DCMotor motor2(2); //左后电机2
AF_DCMotor motor3(3); //右前电机3
AF_DCMotor motor4(4); //右后电机4
const int leftSensor=A0; //左边红外传感器接口
const int rightSensor=A1; //右边红外传感器接口
const int trigPin=9; //超声波传感器 trigPin
const int echoPin=10; //超声波传感器 echoPin
const int ledPin=13; //LED灯 接口
void setup()
{
pinMode(leftSensor, INPUT); //初始化左边红外传感器
pinMode(rightSensor, INPUT); //初始化右边红外传感器
pinMode(trigPin, OUTPUT); //初始化超声波传感器 trigPin
pinMode(echoPin, INPUT); //初始化超声波传感器 echoPin
pinMode(ledPin, OUTPUT); //初始化LED灯
motor1.setSpeed(speed); //设置左前电机1速度
motor2.setSpeed(speed); //设置左后电机2速度
motor3.setSpeed(speed); //设置右前电机3速度
motor4.setSpeed(speed); //设置右后电机4速度
motor1.run(RELEASE);
motor2.run(RELEASE);
motor3.run(RELEASE);
motor4.run(RELEASE);
}
void loop()
{
int leftValue = digitalRead(leftSensor); //读取左边红外传感器返回值
int rightValue = digitalRead(rightSensor); //读取右边红外传感器返回值
int distance = getDistance(); //读取超声波传感器距离
if (leftValue == 1 && rightValue == 0) //沿着线条往左转，右边电机转速高
{
motor1.run(FORWARD);
motor2.run(FORWARD);
motor3.run(RELEASE);
motor4.run(FORWARD);
}
else if (leftValue == 0 && rightValue == 1) //沿着线条往右转，左边电机转速高
{
motor1.run(FORWARD);
motor2.run(RELEASE);
motor3.run(FORWARD);
motor4.run(FORWARD);
}
else if(distance<20) //判断障碍物，距离小于20cm
{
digitalWrite(ledPin, HIGH); //点亮LED灯
motor1.run(FORWARD); //车子右转优先避障
motor2.run(FORWARD);
motor3.run(FORWARD);
motor4.run(RELEASE);
delay(1000);
//返回黑色线轨迹上
motor1.run(FORWARD);
motor2.run(FORWARD);
motor3.run(RELEASE);
motor4.run(FORWARD);
}
else //沿着线条直线行驶
{
digitalWrite(ledPin, LOW); //关闭LED灯
motor1.run(FORWARD);
motor2.run(FORWARD);
motor3.run(FORWARD);
motor4.run(FORWARD);
}
}
int getDistance() //超声波传感器获取距离函数
{
digitalWrite(trigPin, LOW);
delayMicroseconds(2); //低电平两微秒
digitalWrite(trigPin, HIGH);
delayMicroseconds(10); //高电平10微秒
digitalWrite(trigPin, LOW);
int distance = pulseIn(echoPin, HIGH) / 58.00;
return distance;
}