写一个arduino程序,控制车(四个电机驱动轮)实现循迹避障功能,要求如下:1.两个红外传感器辅助车实现沿着单一黑色线行驶;2.超声波传感器(HC-SR04)判断前方是否有障碍,如果有障碍,车子优先避障,然后绕过障碍物之后再次回到黑色线轨迹上。3.配备光敏传感器控制LED灯,外界环境亮度不够时点亮LED。备注:左边两个轮子同步运转,右边两个轮子运动同步,利用左右的差速实现车身偏转,四个轮子由电机驱动模块L9110驱动。
时间: 2023-05-24 12:03:38 浏览: 246
A:
以下是一个简单的Arduino程序,用于控制车辆实现循迹避障功能:
```
#include "L9110.h" // 电机驱动库
#include <Ultrasonic.h> // 超声波传感器库
#include <QTRSensors.h> // 红外传感器库
// 定义电机驱动模块引脚
#define MOTOR_LEFT_A 5
#define MOTOR_LEFT_B 6
#define MOTOR_RIGHT_A 9
#define MOTOR_RIGHT_B 10
// 定义红外传感器引脚
#define PIN_SENSOR1 A0
#define PIN_SENSOR2 A1
#define NUM_SENSORS 2
#define TIMEOUT 2500
// 定义超声波传感器引脚
#define TRIGGER_PIN A2
#define ECHO_PIN A3
#define MAX_DISTANCE 200
// 定义光敏传感器引脚
#define PIN_LIGHT_SENSOR A4
#define PIN_LED 3
// 创建 L9110 对象
L9110 motor_left(MOTOR_LEFT_A, MOTOR_LEFT_B);
L9110 motor_right(MOTOR_RIGHT_A, MOTOR_RIGHT_B);
// 创建 Ultrasonic 对象
Ultrasonic ultrasonic(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
// 创建 QTRSensorsAnalog 对象
QTRSensorsAnalog sensors((unsigned char[]) {PIN_SENSOR1, PIN_SENSOR2}, NUM_SENSORS, TIMEOUT);
// 创建变量
int sensor_values[NUM_SENSORS];
int max_sensor_value;
int sum;
int last_error;
int led_value;
// 初始化函数
void setup() {
pinMode(PIN_LED, OUTPUT);
digitalWrite(PIN_LED, HIGH);
//等待外部输入,初始化运行设置
while(!Serial);
Serial.begin(9600);
// 红外传感器校准
int i;
Serial.println("Calibrating...");
for(i = 0; i < 400; i++) {
sensors.calibrate();
delay(20);
}
Serial.println("Calibration done.");
// 设置电机速度
motor_left.setSpeed(150);
motor_right.setSpeed(150);
// 初始化变量
max_sensor_value = 0;
last_error = 0;
led_value = 0;
}
// 循环函数
void loop() {
// 读取红外传感器数据
sensors.read(sensor_values);
// 计算传感器平均值
int avg = 0;
for(int i = 0; i < NUM_SENSORS; i++) {
avg += sensor_values[i];
if(sensor_values[i] > sensor_values[max_sensor_value]) {
max_sensor_value = i;
}
}
avg = avg / NUM_SENSORS;
// 计算偏差
int error = max_sensor_value - 0.5 * (NUM_SENSORS - 1);
// 计算偏差微调量
int error_delta = error - last_error;
last_error = error;
// 计算偏差积分量
sum += error;
if(sum > 1000) {
sum = 1000;
}
if(sum < -1000) {
sum = -1000;
}
// 更新电机速度
int left_speed = 150 + (error * 30) + (sum * 0.1) + (error_delta * 50);
int right_speed = 150 - (error * 30) - (sum * 0.1) - (error_delta * 50);
motor_left.setSpeed(left_speed);
motor_right.setSpeed(right_speed);
// 控制车辆行驶方向
if(avg < 1000) {
motor_left.stop();
motor_right.stop();
} else {
motor_left.forward();
motor_right.forward();
}
// 判断前方是否有障碍
int distance = ultrasonic.Ranging(CM);
if(distance < 20) {
// 左拐绕过障碍
motor_left.stop();
motor_right.setSpeed(150);
motor_right.forward();
delay(1000);
// 往回行进
motor_left.setSpeed(150);
motor_left.backward();
motor_right.setSpeed(150);
motor_right.backward();
delay(1000);
// 右拐绕过障碍
motor_left.setSpeed(150);
motor_left.forward();
motor_right.stop();
delay(1000);
// 再次回到黑色线轨迹上
motor_left.setSpeed(150);
motor_left.backward();
motor_right.setSpeed(150);
motor_right.backward();
delay(1000);
}
// 根据光敏传感器数据控制 LED 灯
int light_value = analogRead(PIN_LIGHT_SENSOR);
if(light_value < 300) {
led_value = 255;
} else {
led_value = 0;
}
analogWrite(PIN_LED, led_value);
// 打印调试信息
Serial.print(sensor_values[0]);
Serial.print(" ");
Serial.print(sensor_values[1]);
Serial.print(" ");
Serial.print(distance);
Serial.print(" ");
Serial.print(left_speed);
Serial.print(" ");
Serial.println(right_speed);
// 稍作延迟
delay(50);
}
```
注意,在使用此程序时,需要先将 L9110、Ultrasonic 和 QTRSensors 库导入到 Arduino 中。你可以通过 Arduino IDE 菜单中的 "Sketch" -> "Include Library" -> "Manage Libraries" 来安装这些库。
此外,还需要按照代码中的引脚定义,将电机、红外传感器、超声波传感器和光敏传感器等模块正确连接到 Arduino 上。