#include<reg52.h>
#include<intrins.h>
#define uchar unsigned char
#define uint unsigned int
uint Motor_counter,speed_H,speed_L,speedleft,speedright,Lv,Rv;
uint v1=1500,v2=1500,IN_counter=20;
uint k=50;
int yichu=1500;
sbit left=P1^0; // left
sbit right=P1^1;
sbit IN1=P2^1;
sbit IN2=P2^2;
sbit IN=P2^3;
sbit tirg1=P3^5;
sbit tirg2=P3^6;
sbit tirg3=P3^7;
sbit tirg4=P1^5;
sbit tirg5=P1^6;
sbit echo=P3^2;
unsigned int tf,ts;
int i=0;
float distance1,distance2,distance3,distance4,distance5;
void delay10us()
{
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
}
void init(uint s)
{
TMOD|=0x01;
TH0=(65536-s)/256;
TL0=(65536-s)%256;
TR0=1;
ET0=1;
EA=1;
PT0=1;
EX0=1;
IT0=1;
}
void send1(void)
{
tirg1=1;
delay10us();
tirg1=0;
while(echo==0);
tf=(TH0<<8)|TL0;
}
void send2(void)
{
tirg2=1;
delay10us();
tirg2=0;
while(echo==0);
tf=(TH0<<8)|TL0;
}
void send3(void)
{
tirg3=1;
delay10us();
tirg3=0;
while(echo==0);
tf=(TH0<<8)|TL0;
}
void send4(void)
{
tirg4=1;
delay10us();
tirg4=0;
while(echo==0);
tf=(TH0<<8)|TL0;
}
void send5(void)
{
tirg5=1;
delay10us();
tirg5=0;
while(echo==0);
tf=(TH0<<8)|TL0;
}
void shou1()
{
ts=(TH0<<8)|TL0;
distance1=(ts-tf)*0.017;
}
void shou2()
{
ts=(TH0<<8)|TL0;
distance2=(ts-tf)*0.017;
}
void shou3()
{
ts=(TH0<<8)|TL0;
distance3=(ts-tf)*0.017;
}
void shou4()
{
ts=(TH0<<8)|TL0;
distance4=(ts-tf)*0.017;
}
void shou5()
{
ts=(TH0<<8)|TL0;
distance5=(ts-tf)*0.017;
}
void delay()
{ uint i,j;
for(i=10;i>0;i--)
for(j=4500;j>0;j--);
}
void main()
{
init(10000);
TMOD|=0X10;
TH1=(65536-90)/256;
TL1=(65536-90)%256;
TR1=1;
PT1=1;
ET1=1;
EA=1;
while(1)
{
// if(IN1==1&&IN2==1)
// {
// if(distance1<20){Rv=-200;Lv=200; }
// else
// {if(distance3<30||distance5<25){Rv=-200;Lv=-200; }
// if(distance2<30||distance4<25){Rv=200;Lv=200; }
// // else if(distance4<25){Rv=-200;Lv=200; }
// // else if(distance5<25){Rv=200;Lv=-200; }
// else {Rv=-200;Lv=200;}
// }
// }
// if(IN1==1&&IN2==0)
// {
// Rv=200;Lv=200;delay();
// }
// if(IN1==0&&IN2==1)
// {
// Rv=-200;Lv=-200;delay();
// }
// if(IN1==0&&IN2==0)
// {
// Rv=200;Lv=200;delay();
// }
if(IN1==1&&IN2==1)
{
if(distance1<20){Rv=-200;Lv=200; }
else
{if(distance3<50||distance5<25){Rv=-200;Lv=-200; }
if(distance2<50||distance4<25){Rv=200;Lv=200; }
// else if(distance4<25){Rv=-200;Lv=200; }
// else if(distance5<25){Rv=200;Lv=-200; }
else {Rv=-200;Lv=200;}
}
}
if(IN1==1&&IN2==0)
{
Rv=200;Lv=200;delay();
}
if(IN1==0&&IN2==1) //3Î′¼ì2aμ½ê±£¬Æ«à룬×ó×a
{
Rv=-200;Lv=-200;delay();
}
if(IN1==0&&IN2==0)
{
Rv=200;Lv=200;delay();
}
}
}
void Timer0Interrupt(void) interrupt 3
{
speedleft=1382+Lv;
speedright=1382+Rv;
TH1=speed_H;
TL1=speed_L;
Motor_counter++;
if(Motor_counter>7)Motor_counter=0;
switch(Motor_counter)
{
case 0:speed_H=0xf1;speed_L=0x9a; left=1; break;
case 1:
case 2:
case 3:speed_H=(0xffff-speedright)>>8;speed_L=(0xffff-speedright)&0x00ff;left=0;break;
case 4:speed_H=0xf1;speed_L=0x9a;right=1;break;
case 5:
case 6:
case 7:speed_H=(0xffff-speedleft)>>8;speed_L=(0xffff-speedleft)&0x00ff;right=0;break;
default:break;
}
}
void Timer1() interrupt 1
{
TH0=(65536-13000)/256;
TL0=(65536-13000)%256;
// ec1=~ec1;
i++;
if(i==0)
send1();
if(i==1)
send2();
if(i==2)
send3();
if(i==3)
send4();
if(i==4)
send5();
if(i==5)
{
i=0;
send1();
}
}
void exter0() interrupt 0
{
if(ts>65535)
ts=65000;
else
if(i==0)
shou1();
if(i==1)
shou2();
if(i==2)
shou3();
if(i==3)
shou4();
if(i==4)
shou5();
}
|