超声波避障小车+舵机

#include<reg52.h>

#include<intrins.h>

#define uchar unsigned char

#define uint unsigned int

#define barrierDis 15

uint sum;

float L;

sbit LED = P0^0;

sbit Trig = P1^5; //??????TRIG

sbit Echo = P1^6; //??????ECHO

sbit motorDriver_1 =P2^1; //???????,????????L298n???IN1

sbit motorDriver_2 =P2^2; //???????,????????L298n???IN2

sbit motorDriver_3 =P2^5; //???????,????????L298n???IN3

sbit motorDriver_4 =P2^4; //???????,????????L298n???IN4

sbit servorControl =P1^7; //???????

uchar flag=0;

uchar control=5;

uchar servorTime=0;

uchar lFlag=0;//???????????

uchar rFlag=0;//???????????

void delay(uchar time){ //????

uchar i;

for(;time>0;time--){

for(i=0;i<255;i++);

}

}

void stop(){ //????

motorDriver_1=0;

motorDriver_2=0;

motorDriver_3=0;

motorDriver_4=0;

}

void turnLeft(){ //?????

motorDriver_1=1;

motorDriver_2=0;

motorDriver_3=0;

motorDriver_4=0;

} //?????

void turnRight(){

motorDriver_1=0;

motorDriver_2=0;

motorDriver_3=1;

motorDriver_4=0;

}

void turnUp(){ //?????

motorDriver_1=1;

motorDriver_2=0;

motorDriver_3=1;

motorDriver_4=0;

}

void turnDown(){ //?????

motorDriver_1=0;

motorDriver_2=1;

motorDriver_3=0;

motorDriver_4=1;

}

void openHc(){

Trig=1;

_nop_();

_nop_();

_nop_();

_nop_();

_nop_();

_nop_();

_nop_();

_nop_();

_nop_();

_nop_();

_nop_();

_nop_();

_nop_();

Trig=0;

}

uchar getDistance(){ //??????????????

TH0=0;

TL0=0;

openHc();

while(!Echo);

TR0=1;

while(Echo);

TR0=0;

sum=TH0*256+TL0;

L=(sum*1.78)/100;

if(L<barrierDis&&flag==0){

LED=0;

return 0;

}

else{

LED=1;

flag=0;

return 1;

}

}

void main(){

LED=1;

EA=1;

ET0=1;

ET1=1;

TMOD=0x11;

TH1=0xff;

TL1=0x9c;

servorTime=0;

while(1){

stop();

control=15;//??????????????

servorTime=0;

TR1=1;

delay(200);

TR1=0;

turnUp();

LED=0;

delay(200);

LED=1;

while(getDistance()==1); //????,????????

do{

stop();

control=22; //???????

servorTime=0;

TR1=1;

delay(200);

TR1=0;

rFlag=getDistance();

control=5; //???????

servorTime=0;

TR1=1;

delay(200);

TR1=0;

lFlag=getDistance();

turnDown();

delay(220);

delay(220);

delay(220);

}while(lFlag==0&&rFlag==0);

if(rFlag==1&&lFlag==0) //???????

turnLeft();

else if(rFlag==0&&lFlag==1)//???????

turnRight();

else if(rFlag==1&&lFlag==1)//????????,?????

turnLeft();

delay(220);

delay(220);

delay(200);

}

}

void T1_int(void) interrupt 3{ //??????????

TH1=0xff;

TL1= 0x9c;

servorTime++;

if(servorTime<=control)

servorControl=1;

else

servorControl=0;

if(servorTime>=200)

servorTime=0;

}

void T0_int(void) interrupt 1{ //?????????

flag=1;

}