#include <reg52.h>
#include <intrins.h>
sbit Trig2 = P2^5;
sbit Echo2 = P2^4;
sbit Trig1 = P2^7;
sbit Echo1 = P2^6;
sbit Trig3 = P2^3;
sbit Echo3 = P2^2;
sbit AIN1 = P0^0;
sbit AIN2 = P0^1;
sbit PWMA = P1^0;
sbit BIN1 = P0^2;
sbit BIN2 = P0^3;
sbit PWMB = P1^1;
sbit AIN1_Front = P0^5;
sbit AIN2_Front = P0^4;
sbit PWMA_Front = P1^2;
sbit BIN1_Front = P0^7;
sbit BIN2_Front = P0^6;
sbit PWMB_Front = P1^3;
#define STOP_DISTANCE 20
#define MAX_DISTANCE 400
void Delay10us(unsigned int us)
{
while(us--) {
_nop_(); _nop_(); _nop_(); _nop_(); _nop_();
_nop_(); _nop_(); _nop_(); _nop_(); _nop_();
}
}
void Timer0_Init(void)
{
TMOD &= 0xF0;
TMOD |= 0x01;
TH0 = 0;
TL0 = 0;
ET0 = 0;
TR0 = 0;
}
void Delay_ms(unsigned int ms) {
unsigned int i, j;
for(i = 0; i < ms; i++) {
for(j = 0; j < 120; j++) {
}
}
}
void Delay_ms(unsigned int ms);
unsigned int Get_Distance2(void);
unsigned int Get_Distance1(void);
unsigned int Get_Distance3(void);
void Motor_Stop(void);
void Turn1(void);
void Turn3(void);
void Motor_Forward(void);
unsigned int Get_Distance2(void) {
unsigned int time_val = 0;
unsigned int timeout = 50000;
Trig2 = 1;
Delay10us(2);
Trig2 = 0;
while(!Echo2 && timeout--) {
if(timeout == 0) return MAX_DISTANCE;
}
TR0 = 1;
TH0 = 0;
TL0 = 0;
timeout = 50000;
while(Echo2 && timeout--) {
if(timeout == 0) {
TR0 = 0;
return MAX_DISTANCE;
}
}
TR0 = 0;
time_val = (TH0 << 8) | TL0;
TH0 = 0;
TL0 = 0;
time_val = time_val / 63;
if(time_val > MAX_DISTANCE) time_val = MAX_DISTANCE;
return time_val;
}
unsigned int Get_Distance1 (void) {
unsigned int time_val = 0;
unsigned int timeout = 50000;
Trig1 = 1;
Delay10us(2);
Trig1 = 0;
while(!Echo1 && timeout) {
timeout--;
}
if(timeout == 0) {
TR0 = 0;
return MAX_DISTANCE;
}
TH0 = 0; TL0 = 0; TF0 = 0;
TR0 = 1;
timeout = 50000;
while(Echo1 && timeout) {
timeout--;
}
TR0 = 0;
time_val = (TH0 << 8) | TL0;
TH0 = 0; TL0 = 0;
time_val = time_val / 63;
if(time_val > MAX_DISTANCE)
time_val = MAX_DISTANCE;
return time_val;
}
unsigned int Get_Distance3(void) {
unsigned int time_val = 0;
unsigned int timeout = 50000;
Trig3 = 1;
Delay10us(2);
Trig3 = 0;
while(!Echo3 && timeout) {
timeout--;
}
if(timeout == 0) {
TR0 = 0;
return MAX_DISTANCE;
}
TH0 = 0; TL0 = 0; TF0 = 0;
TR0 = 1;
timeout = 50000;
while(Echo3 && timeout) {
timeout--;
}
TR0 = 0;
time_val = (TH0 << 8) | TL0;
TH0 = 0; TL0 = 0;
time_val = time_val / 63;
if(time_val > MAX_DISTANCE)
time_val = MAX_DISTANCE;
return time_val;
}
void Turn1(void) {
AIN1 = 0; AIN2 = 0; PWMA = 0;
BIN1 = 1; BIN2 = 0; PWMB = 1;
AIN1_Front = 0; AIN2_Front = 1; PWMA_Front = 1;
BIN1_Front = 1; BIN2_Front = 0; PWMB_Front = 1;
}
void Turn3(void) {
AIN1 = 1; AIN2 = 0; PWMA = 1;
BIN1 = 0; BIN2 = 0; PWMB = 0;
AIN1_Front = 1; AIN2_Front = 0; PWMA_Front = 1;
BIN1_Front = 0; BIN2_Front = 1; PWMB_Front = 1;
}
void Motor_Forward(void) {
AIN1 = 1; AIN2 = 0; PWMA = 1;
BIN1 = 1; BIN2 = 0; PWMB = 1;
AIN1_Front = 1; AIN2_Front = 0; PWMA_Front = 1;
BIN1_Front = 1; BIN2_Front = 0; PWMB_Front = 1;
}
void Motor_Stop(void)
{
AIN1 = 0; AIN2 = 0;
BIN1 = 0; BIN2 = 0;
PWMA = 0; PWMB = 0;
AIN1_Front = 0; AIN2_Front = 0;
BIN1_Front = 0; BIN2_Front = 0;
PWMA_Front = 0; PWMB_Front = 0;
}
void main(void) {
unsigned int dist1, dist2, dist3;
Timer0_Init();
Motor_Stop();
Delay_ms(1000);
while(1) {
dist2 = Get_Distance2();
Delay_ms(30);
dist1 = Get_Distance1();
Delay_ms(30);
dist3 = Get_Distance3();
Delay_ms(30);
if(dist2 < STOP_DISTANCE) {
Motor_Stop();
Delay_ms(200);
if(dist1 > dist3) {
Turn1();
Delay_ms(400);
Motor_Stop();
Delay_ms(100);
} else {
Turn3();
Delay_ms(400);
Motor_Stop();
Delay_ms(100);
}
Motor_Forward();
Delay_ms(500);
} else if(dist1 < (STOP_DISTANCE / 2))
{
Turn3();
Delay_ms(100);
} else if(dist3 < (STOP_DISTANCE / 2))
{
Turn1();
Delay_ms(100);
} else {
Motor_Forward();
}
Delay_ms(100);
}
} |
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
