晶振11.0592，有三个超声波避障模块，想将它做成能够智能左右避障的，但是代码一直有问题，不知道是哪错了，
#include <REG52.H>
/*-------------------- 引脚定义 --------------------*/
// 后轮电机
sbit AIN1_L = P0^0; // 左轮方向1
sbit AIN2_L = P0^1; // 左轮方向2
sbit PWMA_L = P1^0; // 左轮 PWM
sbit BIN1_R = P0^2; // 右轮方向1
sbit BIN2_R = P0^3; // 右轮方向2
sbit PWMB_R = P1^1; // 右轮 PWM
// 前轮电机
sbit AIN1_FL = P0^5; // 前左轮方向1
sbit AIN2_FL = P0^4; // 前左轮方向2
sbit PWMA_FL = P1^2; // 前左轮 PWM
sbit BIN1_FR = P0^7; // 前右轮方向1
sbit BIN2_FR = P0^6; // 前右轮方向2
sbit PWMB_FR = P1^3; // 前右轮 PWM
// 超声波传感器引脚
sbit TRIG_M = P2^5; // 中间超声波Trig
sbit ECHO_M = P2^4; // 中间超声波Echo
sbit TRIG_L = P2^7; // 左侧超声波Trig
sbit ECHO_L = P2^6; // 左侧超声波Echo
sbit TRIG_R = P2^3; // 右侧超声波Trig
sbit ECHO_R = P2^2; // 右侧超声波Echo
/*-------------------- 参数定义 --------------------*/
#define SAFE_DISTANCE 30 // 安全距离30cm
#define TURN_TIME 400 // 转向时间400ms
/*-------------------- 延时函数 --------------------*/
void Delay(unsigned int count) {
unsigned int i, j;
for(i = 0; i < count; i++) {
for(j = 0; j < 100; j++);
}
}
/*-------------------- 超声波测距函数 --------------------*/
unsigned int GetDistance_Mid(void) {
unsigned int time = 0;
// 发送触发信号
TRIG_M = 0;
TRIG_M = 1;
Delay(1); // 短暂延时
TRIG_M = 0;
// 等待回波信号
while(!ECHO_M);
// 测量高电平时间
while(ECHO_M) {
Delay(1);
time++;
if(time > 1000) break; // 超时保护
}
return time;
}
unsigned int GetDistance_Left(void) {
unsigned int time = 0;
TRIG_L = 0;
TRIG_L = 1;
Delay(1);
TRIG_L = 0;
while(!ECHO_L);
while(ECHO_L) {
Delay(1);
time++;
if(time > 1000) break;
}
return time;
}
unsigned int GetDistance_Right(void) {
unsigned int time = 0;
TRIG_R = 0;
TRIG_R = 1;
Delay(1);
TRIG_R = 0;
while(!ECHO_R);
while(ECHO_R) {
Delay(1);
time++;
if(time > 1000) break;
}
return time;
}
/*-------------------- 电机控制函数 --------------------*/
void Motor_Stop(void) {
PWMA_L = 0; PWMB_R = 0;
PWMA_FL = 0; PWMB_FR = 0;
}
void Motor_Forward(void) {
// 后轮前进
AIN1_L = 1; AIN2_L = 0; PWMA_L = 1;
BIN1_R = 1; BIN2_R = 0; PWMB_R = 1;
// 前轮直行
AIN1_FL = 0; AIN2_FL = 0; PWMA_FL = 1;
BIN1_FR = 0; BIN2_FR = 0; PWMB_FR = 1;
}
void Motor_TurnLeft(void) {
// 后轮：右轮前进，左轮停止
AIN1_L = 0; AIN2_L = 0; PWMA_L = 0;
BIN1_R = 1; BIN2_R = 0; PWMB_R = 1;
// 前轮：向左转向
AIN1_FL = 0; AIN2_FL = 1; PWMA_FL = 1;
BIN1_FR = 1; BIN2_FR = 0; PWMB_FR = 1;
}
void Motor_TurnRight(void) {
// 后轮：左轮前进，右轮停止
AIN1_L = 1; AIN2_L = 0; PWMA_L = 1;
BIN1_R = 0; BIN2_R = 0; PWMB_R = 0;
// 前轮：向右转向
AIN1_FL = 1; AIN2_FL = 0; PWMA_FL = 1;
BIN1_FR = 0; BIN2_FR = 1; PWMB_FR = 1;
}
/*-------------------- 读取所有传感器距离 --------------------*/
void Read_All_Distances(unsigned int *mid, unsigned int *left, unsigned int *right) {
*mid = GetDistance_Mid();
Delay(30);
*left = GetDistance_Left();
Delay(30);
*right = GetDistance_Right();
Delay(30);
}
/*-------------------- 主函数 --------------------*/
void main(void) {
unsigned int disM, disL, disR;
// 初始化电机为停止状态
Motor_Stop();
Delay(1000); // 上电延时
while(1) {
// 读取三个方向的距离
Read_All_Distances(&disM, &disL, &disR);
// 简单避障逻辑
if(disM < SAFE_DISTANCE) {
// 前方有障碍物，停车
Motor_Stop();
Delay(200);
// 比较左右距离，选择更宽敞的一侧
if(disL > disR) {
// 向左转
Motor_TurnLeft();
Delay(TURN_TIME);
} else {
// 向右转
Motor_TurnRight();
Delay(TURN_TIME);
}
// 转向后前进
Motor_Forward();
Delay(500);
} else {
// 前方安全，直行前进
Motor_Forward();
}
Delay(100); // 主循环延时
}
} |
QQ好友和群
QQ空间
腾讯微博
腾讯朋友
收藏
淘帖
顶
踩
