int Echo = A0; // Echo回声脚(P2.0)
int Trig = A1; // Trig 触发脚(P2.1)
// 红外避障传感器引脚定义
int IR_left = 2; // 左侧红外传感器（检测到障碍物时输出LOW）
int IR_right = 3; // 右侧红外传感器（检测到障碍物时输出LOW）
int F_Distance = 0;
int L_Distance = 0;
int R_Distance = 0;
int L_motor_back = 5; //左电机后退(IN1)
int L_motor_go = 6; //左电机前进(IN2)
int R_motor_go = 9; //右电机前进(IN3)
int R_motor_back = 10; //右电机后退(IN4)
int servopin = 12; //设置舵机驱动脚到数字口12
int angle; //定义角度变量
int pulsewidth; //定义脉宽变量
int val;
// 红外避障相关变量
bool left_obstacle = false;
bool right_obstacle = false;
bool use_ultrasonic = true; // 是否启用超声波避障
int avoid_delay = 300; // 避障动作持续时间(ms)
void setup()
{
Serial.begin(9600); // 初始化串口
// 初始化电机驱动IO为输出方式
pinMode(L_motor_go, OUTPUT); // PIN 5 (PWM)
pinMode(L_motor_back, OUTPUT); // PIN 6 (PWM)
pinMode(R_motor_go, OUTPUT); // PIN 9 (PWM)
pinMode(R_motor_back, OUTPUT); // PIN 10 (PWM)
// 初始化超声波引脚
pinMode(Echo, INPUT); // 定义超声波输入脚
pinMode(Trig, OUTPUT); // 定义超声波输出脚
// 初始化红外传感器引脚
pinMode(IR_left, INPUT_PULLUP); // 使用内部上拉电阻
pinMode(IR_right, INPUT_PULLUP); // 使用内部上拉电阻
// LCD初始化（如果使用LCD）
// lcd.begin(16,2); //初始化1602液晶工作模式
// 定义1602液晶显示范围为2行16列字符
pinMode(servopin, OUTPUT); //设定舵机接口为输出接口
Serial.println("System Initialized - Ultrasonic + IR Obstacle Avoidance");
}
//=======================智能小车的基本动作=========================
void run() // 前进
{
analogWrite(R_motor_go, 90); //右电机前进
analogWrite(R_motor_back, 0);
analogWrite(L_motor_go, 99); //左电机前进
analogWrite(L_motor_back, 0);
}
void brake() //刹车，停车
{
digitalWrite(R_motor_go, LOW);
digitalWrite(R_motor_back, LOW);
digitalWrite(L_motor_go, LOW);
digitalWrite(L_motor_back, LOW);
}
void left() //左转(左轮不动，右轮前进)
{
analogWrite(R_motor_go, 100); //右电机前进
analogWrite(R_motor_back, 0);
digitalWrite(L_motor_go, LOW); //左轮不动
digitalWrite(L_motor_back, LOW);
}
void spin_left() //左转(左轮后退，右轮前进)
{
analogWrite(R_motor_go, 100); //右电机前进
analogWrite(R_motor_back, 0);
analogWrite(L_motor_go, 0);
analogWrite(L_motor_back, 100); //左轮后退
}
void right() //右转(右轮不动，左轮前进)
{
digitalWrite(R_motor_go, LOW); //右电机不动
digitalWrite(R_motor_back, LOW);
analogWrite(L_motor_go, 100); //左电机前进
analogWrite(L_motor_back, 0);
}
void spin_right() //右转(右轮后退，左轮前进)
{
analogWrite(R_motor_go, 0);
analogWrite(R_motor_back, 100); //右电机后退
analogWrite(L_motor_go, 100); //左电机前进
analogWrite(L_motor_back, 0);
}
void back() //后退
{
analogWrite(R_motor_go, 0);
analogWrite(R_motor_back, 100); //右轮后退
analogWrite(L_motor_go, 0);
analogWrite(L_motor_back, 100); //左轮后退
}
//==========================================================
// 超声波距离测量函数
float Distance_test()
{
digitalWrite(Trig, LOW);
delayMicroseconds(2);
digitalWrite(Trig, HIGH);
delayMicroseconds(10);
digitalWrite(Trig, LOW);
float Fdistance = pulseIn(Echo, HIGH);
Fdistance = Fdistance / 58;
return Fdistance;
}
// 红外传感器检测函数
void IR_Detection()
{
// 读取红外传感器状态（假设LOW表示检测到障碍物）
left_obstacle = (digitalRead(IR_left) == LOW);
right_obstacle = (digitalRead(IR_right) == LOW);
// 可选：串口输出红外传感器状态
Serial.print("IR Left: ");
Serial.print(left_obstacle ? "OBSTACLE" : "CLEAR");
Serial.print(" | IR Right: ");
Serial.println(right_obstacle ? "OBSTACLE" : "CLEAR");
}
// 红外避障决策函数
void IR_Avoidance()
{
IR_Detection(); // 检测红外传感器
if (left_obstacle && right_obstacle) {
// 两侧都有障碍物，后退然后随机转向
Serial.println("Both sides blocked - Back and turn right");
back();
delay(200);
brake();
delay(100);
spin_right();
delay(400);
brake();
}
else if (left_obstacle) {
// 左侧有障碍物，右转
Serial.println("Left obstacle - Turn right");
back();
delay(150);
brake();
delay(100);
right();
delay(avoid_delay);
brake();
}
else if (right_obstacle) {
// 右侧有障碍物，左转
Serial.println("Right obstacle - Turn left");
back();
delay(150);
brake();
delay(100);
left();
delay(avoid_delay);
brake();
}
// 无障碍物时不执行动作
}
// 超声波避障决策函数（原逻辑）
void Ultrasonic_Avoidance()
{
front_detection(); // 测量前方距离
if (F_Distance < 32) // 当遇到障碍物时
{
Serial.print("Front obstacle detected: ");
Serial.print(F_Distance);
Serial.println(" cm");
back(); // 后退减速
delay(200);
brake(); // 停下来做测距
left_detection(); // 测量左边距障碍物距离
right_detection(); // 测量右边距障碍物距离
// Distance_display(L_Distance); // 如果需要显示距离
// Distance_display(R_Distance);
if ((L_Distance < 35) && (R_Distance < 35)) {
// 当左右两侧均有障碍物靠得比较近
Serial.println("Both sides too close - Spin left");
spin_left();
delay(100);
}
else if (L_Distance > R_Distance) {
// 左边比右边空旷
Serial.println("Left clearer - Turn left");
left();
delay(300);
brake();
}
else {
// 右边比左边空旷
Serial.println("Right clearer - Turn right");
right();
delay(300);
brake();
}
}
else {
// 无障碍物，直行
run();
}
}
// 舵机控制函数
void servopulse(int servopin, int angle)
{
pulsewidth = (angle * 11) + 500; // 将角度转化为500-2480的脉宽值
digitalWrite(servopin, HIGH);
delayMicroseconds(pulsewidth);
digitalWrite(servopin, LOW);
delay(20 - pulsewidth / 1000);
}
void front_detection()
{
for (int i = 0; i <= 5; i++) {
servopulse(servopin, 90);
}
F_Distance = Distance_test();
Serial.print("Front Distance: ");
Serial.println(F_Distance);
}
void left_detection()
{
for (int i = 0; i <= 15; i++) {
servopulse(servopin, 175);
}
L_Distance = Distance_test();
Serial.print("Left Distance: ");
Serial.println(L_Distance);
}
void right_detection()
{
for (int i = 0; i <= 15; i++) {
servopulse(servopin, 5);
}
R_Distance = Distance_test();
Serial.print("Right Distance: ");
Serial.println(R_Distance);
}
// 综合避障主循环
void loop()
{
while (1)
{
// 首先进行红外避障检测（近距离优先）
IR_Detection();
if (left_obstacle || right_obstacle) {
// 红外检测到障碍物，执行红外避障
IR_Avoidance();
}
else {
// 红外没有检测到障碍物，执行超声波避障
Ultrasonic_Avoidance();
}
// 可选：添加短暂延迟，避免过于频繁的检测
delay(50);
}
}
// 可选：LCD显示函数（如果使用LCD）
void Distance_display(int Distance)
{
/*
if ((2 < Distance) && (Distance < 400))
{
lcd.home();
lcd.print(" Distance: ");
lcd.setCursor(6, 2);
lcd.print(Distance);
lcd.print("cm");
}
else
{
lcd.home();
lcd.print("!!! Out of range");
}
delay(250);
lcd.clear();
*/
}
|
