|
|
#include <STC89C5xRC.H>
#include <intrins.h>
typedef unsigned char u8;
typedef unsigned int u16;
sbit ENA=P0^5;
sbit IN1=P0^4;
sbit IN2=P0^3;
sbit IN3=P0^2;
sbit IN4=P0^1;
sbit ENB=P0^0;
sbit Echo=P3^3;
sbit Trig=P3^4;
sbit infrared_L=P3^7;
sbit infrared_R=P3^6;
u8 PWM_left=0;
u8 PWM_right=0;
u8 PWM_T=0; //PWM周期
u16 time=0;
void front();
void back();
void turnleft();
void turnright();
void stop();
unsigned long S=0;
bit flag=0;
void delay21us() //誤差 0us
{
unsigned char a;
for(a=9;a>0;a--);
}
void delay10ms() //誤差 0us
{
unsigned char a,b,c;
for(c=1;c>0;c--)
for(b=38;b>0;b--)
for(a=130;a>0;a--);
}
void delay80ms() //誤差 0us
{
unsigned char a,b,c;
for(c=2;c>0;c--)
for(b=95;b>0;b--)
for(a=209;a>0;a--);
_nop_();
}
void delay400ms() //誤差 0us
{
unsigned char a,b,c;
for(c=29;c>0;c--)
for(b=70;b>0;b--)
for(a=97;a>0;a--);
}
void timer0() interrupt 1
{
flag=1;
Echo=0;
}
void Timer1Init()
{
TMOD|=0x10;
TH1=0xfc;
TL1=0x18;
TR1=1;
ET1=1;
EA=1;
}
void Timer1()interrupt 3
{
TH1=0xfc;
TL1=0x18;
PWM_T++;
if(PWM_T<=PWM_left)
ENA=1;
if(PWM_T<=PWM_right)
ENB=1;
if(PWM_T>=10)
PWM_T=0;
}
void ultrasound()
{
Trig=1;
delay21us();
Trig=0;
}
void ultrasound_range() //超聲波避障部分
{
time=TH0*256+TL0;
TH0=0;
TL0=0;
S=time*0.17;
if(S<400)
{
back();
delay80ms();
delay80ms();
turnright();
}
else
{
front();
}
if((S>=4000)||flag==1)
{
flag=0;
}
}
void front()
{
PWM_left=0;
PWM_right=0;
IN1=1;IN2=0;
IN3=1;IN4=0;
}
void back()
{
PWM_left=3;
PWM_right=3;
IN1=0;IN2=1;
IN3=0;IN4=1;
}
void turnleft()
{
PWM_left=3;
PWM_right=3;
IN1=0;IN2=1;
IN3=1;IN4=0;
delay400ms();
IN2=0;
}
void turnright()
{
PWM_left=3;
PWM_right=3;
IN1=1;IN2=0;
IN3=0;IN4=1;
delay400ms();
IN4=0;
}
void stop()
{
PWM_left=9;
PWM_right=9;
IN1=0;IN2=0;
IN3=0;IN4=0;
}
void IOA() //紅外避障部分
{
if(infrared_L==1&&infrared_R==1&&(S>400))
{
delay10ms();
front();
}
if(infrared_L==0&&infrared_R==1)
{
turnright();
}
if(infrared_L==1&&infrared_R==0)
{
turnleft();
}
if(infrared_L==0&&infrared_R==0)
{
stop();
delay10ms();
back();
delay10ms();
}
}
void main()
{
TMOD|=0x01;
EA=1;
TH0=0;
TL0=0;
ET0=1;
Timer1Init();
while(1)
{
IOA();
ultrasound();
while(!Echo);
TR0=1;
while(Echo);
TR0=0;
ultrasound_range();
}
}
|
|
