如何將這個89c52程序改為stc15f2k60s2超聲波舵機(jī)避障程序

ID:521608 · 發(fā)表于 2019-5-14 17:27

#include<reg52.h>
#include<intrins.h>
#include  "bizhang.h"
#define u8 unsigned char
#define u32 unsigned int
#define barrierDis 0.2
sbit IN1 =P0^1;
sbit IN2 =P0^0;
sbit IN3 =P0^2;
sbit IN4 =P0^3;
sbit Trig  = P0^5;
sbit Echo  = P0^4;
sbit servorControl =P0^6;
u8 flag=0;
u32 jieshoutime;
float juli;
u8 control=5;
u8 servorTime=0;
u8 leftFlag=0;
u8 rightFlag=0;
void bizhang_delay_ms(u32 ms)
{
  u8 time;
  while(ms--)
  for(time=0;time<100;time++);
}
void chufa()
{
Trig=1;
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
_nop_();
Trig=0;
}
void stop()
{
IN1=0;
IN2=0;
IN3=0;
IN4=0;
}
void forward()
{
IN1=0;
IN2=1;
IN3=0;
IN4=1;
}
void anticlockwise()
{
IN1=0;
IN2=1;
IN3=1;
IN4=0;
}
void backoff ()
{
IN1=1;
IN2=0;
IN3=1;
IN4=0;
}
void clockwise()
{
IN1=1;
IN2=0;
IN3=0;
IN4=1;
}
u8 getDistance()
{
TH0=0;
TL0=0;
chufa();
while(!Echo);
TR0=1;
while(Echo);
TR0=0;
jieshoutime=TH0*256+TL0;
juli=(jieshoutime*0.000001*340)/2;
if(juli<barrierDis&&flag==0)
{
   return 0;
}
else
{
      flag=0;
      return 1;
}
}

void  xiaoche_bizhang()
{
EA=1;
ET0=1;
ET1=1;
TMOD=0x11;
TH1=0xff;
TL1=0x9c;
servorTime=0;
while(1)
{
  stop();
  bizhang_delay_ms(200);
  control=10;
  servorTime=0;
  TR1=1;
  bizhang_delay_ms(100);
  TR1=0;
  forward();
  while(getDistance()==1);
  do
  {
stop();
control=15;
servorTime=0;
TR1=1;
bizhang_delay_ms(100);
TR1=0;
leftFlag=getDistance();
control=5;
servorTime=0;
TR1=1;
bizhang_delay_ms(100);
TR1=0;
rightFlag=getDistance();
control=10;
servorTime=0;
TR1=1;
bizhang_delay_ms(100);
TR1=0;
backoff ();
bizhang_delay_ms(200);
  }
  while(leftFlag==0&&rightFlag==0);
  if(rightFlag==0&&leftFlag==1)
   anticlockwise();
  else if(rightFlag==1&&leftFlag==0)
  clockwise();
  else if(rightFlag==1&&leftFlag==1)
  anticlockwise();
  bizhang_delay_ms(1000);
}
}

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;
}