晶振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); // 主循環延時
}
} |
