pp.c

控制智能小车寻找跷跷板的平衡位置

#include <reg52.H>
#include <stdio.h>
#include <intrins.h>
#include <stdlib.h>
#define uchar unsigned char
#define uint unsigned int
void    delay(uint i);
// ================= 引脚定义 =======================
sbit  IN1 = P1^0;  //电机
sbit  IN2 = P1^1;  //电机
sbit  IN3 = P1^2;  //电机
sbit  IN4 = P1^3;  //电机
sbit  youx= P1^4; 
sbit  zuox= P1^5;
sbit  bz  = P1^6;
sbit  fm  = P1^7;
uint  flag=0;
uint h_leve=0;
uint l_leve=0;
uint  time=500;
uint pwm=500;
uchar second=0; 
uint  n=0; 
void interrupt_init(void)
{
	TMOD=0x01; 
	TH0=(65535-1000)/256;
	TL0=(65535-1000)%256 ; 
        EA =1;
        ET0=1;
	    TR0=1;
		IN1=1;
		IN2=1;
		IN3=1;
		IN4=1;
		EX0=0;
		EX1=0;
}             
//==================前进==================
void go1(void)
{	  
  pwm=200; 
  IN1=1;IN2=0;
  IN3=1;IN4=0; 
}
//==================前进==================
void go2(void)
{	 
 pwm=150; 
  IN1=1;IN2=0;
  IN3=1;IN4=0; 
}


//==================后退==================
void back1(void)				
{                
pwm=200;
IN1=0; IN2=1; 
IN3=0; IN4=1;
}
void back2(void)				
{         
pwm=80;
IN1=0; IN2=1; 
IN3=0; IN4=1;
}
//==================左转（两电机转向相反）==================
void left(void)          
{	
pwm=230;
IN1=1;IN2=0; 
IN3=0;IN4=1;
}
//==================右转（两电机转向相反）==================
void right(void)         
{ 	 
pwm=200;
IN1=0;IN2=1;
IN3=1;IN4=0;
}
//==================制动==================
void stop(void)
{  
 pwm=0;
 IN1=0;IN2=0;
 IN3=0;IN4=0;
}   
//==================避障==================
void BZ(void)
{
   back2();
delay(1000);
   right();
   delay(500);
   go2();
   delay(1000);
}

void delay(uint i)//延时
{
 uint idata  a;
 uchar idata b;
 for(a=0;a<i;a++)
 for(b=0;b<227;b++)
  {
   _nop_();
  }
}

void delay800ms(void)
{
 uchar i=10;
 uint j;
 while(i--)
 {j=1817;
  while(j--);
 }
}

void delay5ms(void)
{uint i=5552;
 while(i--);
}
void  timeinterrupt1(void) interrupt 1
   {
	TH0=(65535-100)/256;
	TL0=(65535-100)%256;
    n++;
    if(n==10000) {second++;n=0;}		
	if(h_leve)
		{
		 h_leve--;
		 IN1=1;
		 IN3=1;	
		 return;
		}
		 if(l_leve)
		{
		 l_leve--;
	     IN1=0;
		 IN3=0;
		 return;
		}
		h_leve=pwm;
		l_leve=time-pwm;
}

void main(void)
{ 
interrupt_init();
     fm=0;
	 go2();
	 delay(100);       
	while(1)
	{
    youx=1;zuox=1;
	loop1:
   while(!(youx||zuox))
	{     
        bz=1;   
        go1();
		delay(300);
        if(bz==0)BZ();
        delay(100);
        right();
        delay(300);
        if(bz==0)BZ();
        if(zuox||youx)goto loop2;
	    if(youx&&zuox)goto loop3;
        stop();
        delay(100);
        if(zuox||youx)goto loop2;
	    if(youx&&zuox)goto loop3;
        left();
        delay(450);
        if(bz==0)BZ();
        if(zuox||youx)goto loop2;
	    if(youx&&zuox)goto loop3;
        stop();
        delay(100);       
        if(zuox||youx)goto loop2;
	    if(youx&&zuox)goto loop3;
        right();
        delay(260);
        if(bz==0)BZ();
        if(zuox||youx)goto loop2;
	    if(youx&&zuox)goto loop3;
        stop();
        delay(100);
        go1();
        delay(500);
        if(bz==0)BZ();
        if(zuox||youx)goto loop2;
	    if(youx&&zuox)goto loop3;
        stop();
        delay(100);
    }
	loop2:
   while(youx!=zuox)
   {
      if(youx&&zuox)goto loop3;
      if(youx){right();delay(60);}
	  stop();
	  delay(50);
	           if(!(youx||zuox))goto loop1;
	           if(youx&&zuox)goto loop3;
	              go2();
				  delay(100);
      if(!(youx||zuox))goto loop1;
	  if(youx&&zuox)goto loop3;
      if(zuox){left();delay(60);
      stop();
	  delay(50);
	            if(!(youx||zuox))goto loop1;
	            if(youx&&zuox)goto loop3;
				go2();delay(100);}
     if(!(youx||zuox))goto loop1;
	    if(youx&&zuox)goto loop3;
   }
   loop3:
   while(youx&&zuox)
  {
    


       go2();
	   if(!(zuox))
	  { right();
	   delay(60);
	   }
	   stop();
	   delay(50);
       if(!(youx))
	  { left();
	   delay(60);
	   }
	   go2();
	   delay(200);
       stop();
	   delay(50);	 
  	if(!(youx||zuox))break; 
    if(bz==0) 
     {
	    fm=1;  
		delay(200);
        stop();
        delay(5000);      
        }
   }

   }
}