#include<REG52.h>
#include <stdio.h> 
#define uint8  unsigned char
#define uint16 unsigned int
//PWM的输出端口，1，2，3，4，5，6号舵机
sbit PWM_OUT0=P2^0;                                   
sbit PWM_OUT1=P2^1;                                    
sbit PWM_OUT2=P2^2;                                    
sbit PWM_OUT3=P2^3;                                    
sbit PWM_OUT4=P2^4;                                    
sbit PWM_OUT5=P2^5;                                    
sbit PWM_OUT6=P2^6;                                    
sbit PWM_OUT7=P2^7;                                    
//信号检测端口
//低电平有效
sbit LED1=P0^0;	                                       
sbit LED2=P0^1;
sbit LED3=P0^2;
sbit LED4=P0^3;
sbit key=P1^0;
void Movementa(void);
void Movementb(void);
void Movementc(void);
void Movementd(void);
void youzhi(void);
void zuozhi(void);
void turn_you(void);
void turn_zuo(void);
//PWM的数据值 
uint16 PWM_Value[8]={1550,1520,1530,1500,1480,1450,1580,1420};
//电机初始化位置，保持直立，初始设置
uint8 order1;         													
uint8 flag1,flag2,flag3,flag_hei,flag_zhu;								
void delay_nus(uint16 a )
{  
	uint16 i;
	uint16 j;
	for(i=0;i<1000;i++)
	for(j=0;j<a;j++);  
}
/*=======================
=========================
    定时器T0的中断服务程序
    一个循环20ms = 8*2.5ms
==========================
=========================*/
void timer0(void) interrupt 1 using 1
{
 	switch(order1)
 	{
 case 1:  PWM_OUT0=1;
          TH0=-PWM_Value[0]/256;
          TL0=-PWM_Value[0]%256;
          break;
 case 2:  PWM_OUT0=0;       
  	  TH0=-(2500-PWM_Value[0])/256;
          TL0=-(2500-PWM_Value[0])%256;
          break;
 case 3:  PWM_OUT1=1;
          TH0=-PWM_Value[1]/256;
          TL0=-PWM_Value[1]%256;
          break;
 case 4:  PWM_OUT1=0;
          TH0=-(2500-PWM_Value[1])/256;
          TL0=-(2500-PWM_Value[1])%256;
          break;
 case 5:  PWM_OUT2=1;
          TH0=-PWM_Value[2]/256;
          TL0=-PWM_Value[2]%256;
          break;
 case 6:  PWM_OUT2=0 ;
          TH0=-(2500-PWM_Value[2])/256;
          TL0=-(2500-PWM_Value[2])%256;
          break;
 case 7:  PWM_OUT3=1;
          TH0=-PWM_Value[3]/256;
          TL0=-PWM_Value[3]%256;
          break;
 case 8:  PWM_OUT3=0;
          TH0=-(2500-PWM_Value[3])/256;
          TL0=-(2500-PWM_Value[3])%256;
          break;
 case 9:  PWM_OUT4=1;
          TH0=-PWM_Value[4]/256;
          TL0=-PWM_Value[4]%256;
          break;
 case 10: PWM_OUT4=0 ;
          TH0=-(2500-PWM_Value[4])/256;
          TL0=-(2500-PWM_Value[4])%256;
          break;
 case 11: PWM_OUT5=1;
          TH0=-PWM_Value[5]/256;
          TL0=-PWM_Value[5]%256;
          break;
 case 12: PWM_OUT5=0 ;
          TH0=-(2500-PWM_Value[5])/256;
          TL0=-(2500-PWM_Value[5])%256;
          break;
 case 13: PWM_OUT6=1;
          TH0=-PWM_Value[6]/256;
          TL0=-PWM_Value[6]%256;
          break;
 case 14: PWM_OUT6=0 ;
          TH0=-(2500-PWM_Value[6])/256;
          TL0=-(2500-PWM_Value[6])%256;
          break;
 case 15: PWM_OUT7=1;
          TH0=-PWM_Value[7]/256;
          TL0=-PWM_Value[7]%256;
          break;
 case 16: PWM_OUT7=0 ;
          TH0=-(2500-PWM_Value[7])/256;
          TL0=-(2500-PWM_Value[7])%256;
          break;
 default : order1=0;	
    	}
    order1++;
}

/*初始化中断*/
void InitPWM(void)
{
   	order1=1;
   	TMOD |=0x01;
   	TH0=-1500/256;	 	//0.5ms
   	TL0=-1500%256;
   	EA=1;
   	ET0=1;
   	TR0=1;
}

void delay(void)
{
  	uint16 i=100;
  	while(i--);
}
			 
////////////行走////////////
void Movementa(void)
{
 	uint16 m,n,p,q,a,b;
 	int i; 
  	m=PWM_Value[0];n=PWM_Value[5]; 
  	p=PWM_Value[1];q=PWM_Value[2];
  	a=PWM_Value[3];b=PWM_Value[4];
 	for(i=90;i>0;i--)        
    	{ 
	 	m=m+2;
	 	n=n+1;
	 	delay();delay();//delay();delay();
	 	PWM_Value[0]=m;
	 	PWM_Value[5]=n;
    	}  
	delay();
 	for(i=180;i>0;i--)        
    	{ 
        p=p-1;
	q=q-1;
	delay();delay();//delay();delay();
	PWM_Value[1]=p;
	PWM_Value[2]=q;
    	} 
	delay();
 	for(i=90;i>0;i--)        
    	{ 
	 	a=a-2;
         	b=b-2;
	 	n=n-1;
	 	m=m-2;
	 	delay();delay();//delay();delay();
	 	PWM_Value[3]=a;
	 	PWM_Value[4]=b;
	 	PWM_Value[5]=n;
	 	PWM_Value[0]=m;
    	}  
	delay();
}

void Movementb(void)
{
 	uint16 m,n,p,q,a,b;
 	int i; 
  	m=PWM_Value[0];n=PWM_Value[5]; 
  	p=PWM_Value[1];q=PWM_Value[2];
  	a=PWM_Value[3];b=PWM_Value[4];
 	for(i=90;i>0;i--)        
    	{ 
     		m=m-1;
     		n=n-2;
     		p=p+2;
     		q=q+2;
	 	delay();delay();//delay();delay();
	 	PWM_Value[0]=m;
	 	PWM_Value[5]=n;
	 	PWM_Value[1]=p;
	 	PWM_Value[2]=q;
    	} 
	delay();
 	for(i=180;i>0;i--)        
    	{ 
     		a=a+2;
     		b=b+2;
     		//n=n+1;
		delay();delay();//delay();delay();
		PWM_Value[3]=a;
		PWM_Value[4]=b;
		//PWM_Value[5]=n;
    	} 
	delay();
 	for(i=90;i>0;i--)        
    	{ 
     		m=m+1;
     		p=p+2;
     		q=q+2;
     		n=n+2;
	 	delay();delay();//delay();delay();
	 	PWM_Value[0]=m;
	 	PWM_Value[1]=p;
	 	PWM_Value[2]=q;
	 	PWM_Value[5]=n;
    	}  
	delay();
}
	
void Movementc(void)
{
 	uint16 m,n,p,q,a,b;
 	int i; 
  	m=PWM_Value[0];n=PWM_Value[5]; 
  	p=PWM_Value[1];q=PWM_Value[2];
  	a=PWM_Value[3];b=PWM_Value[4];
 	for(i=90;i>0;i--)        
    	{ 
     		m=m+2;
     		n=n+1;
     		a=a-2;
     		b=b-2;
	 	delay();delay();//delay();delay();
	 	PWM_Value[0]=m;
	 	PWM_Value[5]=n;
	 	PWM_Value[3]=a;
	 	PWM_Value[4]=b;
    	} 
	delay();
	for(i=180;i>0;i--)        
    	{ 
	 	p=p-2;
         	q=q-2;
	 	delay();delay();//delay();delay();
	 	PWM_Value[1]=p;
	 	PWM_Value[2]=q;
    	}  
	delay();
 	for(i=90;i>0;i--)        
    	{ 
     		n=n-1;
     		a=a-2;
	 	b=b-2;
	 	m=m-2;
	 	delay();delay();//delay();delay();
	 	PWM_Value[5]=n;
	 	PWM_Value[3]=a;
	 	PWM_Value[4]=b;
	 	PWM_Value[0]=m;
    	} 
	delay();
}

///////////信号检测///////////
void jiance(void)
{
	if((LED1==1)|(LED2==1))
	{ 
		youzhi(); 
		delay_nus(20);
		while((LED1==1)|(LED2==1))
		{
 			turn_zuo(); 
			delay_nus(20);
  		}
 		Movementa(); 
		delay_nus(20);
	}
}

void jiance3(void)
{
	if((LED1==1)|(LED2==1))
	{ 
		zuozhi(); 
		delay_nus(20);
		while((LED1==1)|(LED2==1))
		{
 			turn_zuo(); 
			delay_nus(20);
  		}
		Movementa(); 
		delay_nus(20); 
		Movementb(); 
		delay_nus(20);
	}
}

void jiance1(void)
{
	if((LED3==1)|(LED4==1))
	{ 
		zuozhi(); 
		delay_nus(20);
		while(((LED3==1)|(LED4==1)))
		{
 			turn_you(); 
			delay_nus(20);
  		}
 		Movementa(); 
		delay_nus(20); 
		Movementb(); 
		delay_nus(20);
	}
}

 void jiance2(void)
{
	if((LED3==1)|(LED4==1))
	{ 
		youzhi(); 
		delay_nus(20);
		while(((LED3==1)|(LED4==1)))
		{
 			turn_you(); 
			delay_nus(20);
  		}
  		Movementa(); 
		delay_nus(20);
	}
}

//////////右脚在前，变直立///////////
void youzhi(void)
{
 	uint16 m,n,p,q,a,b;
 	int i; 
  	m=PWM_Value[0];n=PWM_Value[5]; 
  	p=PWM_Value[1];q=PWM_Value[2];
  	a=PWM_Value[3];b=PWM_Value[4];
 	for(i=90;i>0;i--)        
    	{ 
     	 	m=m-1;
	 	n=n-2;
     	 	p=p+2;
	 	q=q+2;
	 	delay();delay();//delay();delay();
	 	PWM_Value[0]=m;
	 	PWM_Value[5]=n;
	 	PWM_Value[1]=p;
	 	PWM_Value[2]=q;
    	} 
	delay();
 	for(i=180;i>0;i--)        
    	{ 
	 	a=a+1;
         	b=b+1;
	 	n=n+1;
	 	delay();delay();//delay();delay();
	 	PWM_Value[3]=a;
	 	PWM_Value[4]=b;
	 	PWM_Value[5]=n;
    	} 
	delay();
 	for(i=90;i>0;i--)        
    	{ 
     	 	m=m+1;
	 	delay();delay();//delay();delay();
	 	PWM_Value[0]=m;
    	}	
  	delay();
}

//////////左脚在前，变直立///////////
void zuozhi(void)
{
 	uint16 m,n,p,q,a,b;
 	int i; 
  	m=PWM_Value[0];n=PWM_Value[5]; 
  	p=PWM_Value[1];q=PWM_Value[2];
  	a=PWM_Value[3];b=PWM_Value[4];
 	for(i=90;i>0;i--)        
    	{ 
     	 	m=m+2;
	 	n=n+1;
     	 	a=a-2;
	 	b=b-2;
	 	delay();delay();//delay();delay();
	 	PWM_Value[0]=m;
	 	PWM_Value[5]=n;
	 	PWM_Value[3]=a;
	 	PWM_Value[4]=b;
    	} 
	delay();
 	for(i=180;i>0;i--)        
    	{ 
	 	p=p-1;
         	q=q-1;
	 	m=m-1;
	 	delay();delay();//delay();delay();
	 	PWM_Value[1]=p;
	 	PWM_Value[2]=q;
	 	PWM_Value[0]=m;
    	}
  	delay();
 	for(i=90;i>0;i--)        
    	{ 
    	 	n=n-1;
	 	delay();delay();//delay();delay();
	 	PWM_Value[5]=n;
    	}	
 	delay();
}

///////////踩到右黑直线，左转12.6/////////
void turn_zuo(void)
{
//向左偏身
 	uint16 m,n,p,q,a,b;
 	int i;
   	m=PWM_Value[0];
	n=PWM_Value[5]; 
  	p=PWM_Value[6];
 	for(i=50;i>0;i--)        
    	{ 
	 	m=m+2;
	 	n=n+1;
	 	delay();//delay();//delay();delay();
	 	PWM_Value[0]=m;
	 	PWM_Value[5]=n;
    	}	
 	for(i=100;i>0;i--)        
    	{ 
	 	p=p-1;
	 	delay();//delay();//delay();delay();
	 	PWM_Value[6]=p;
    	}	 
 	for(i=50;i>0;i--)        
    	{ 
	 	m=m-2;
	 	n=n-1;
	 	delay();//delay();//delay();delay();
	 	PWM_Value[0]=m;
	 	PWM_Value[5]=n;
    	}	
	for(i=50;i>0;i--)        
    	{ 
	 	m=m-1;
	 	n=n-2;
	 	delay();//delay();//delay();delay();
	 	PWM_Value[0]=m;
	 	PWM_Value[5]=n;
    	}	
 	for(i=100;i>0;i--)        
    	{ 
	 	p=p+1;
	 	delay();//delay();//delay();delay();
	 	PWM_Value[6]=p;
    	}
 	for(i=50;i>0;i--)        
    	{ 
	 	m=m+1;
	 	n=n+2;
	 	delay();//delay();//delay();delay();
	 	PWM_Value[0]=m;
	 	PWM_Value[5]=n;
    	}	
}

/////////右转/////////////////
void turn_you(void)
{
//向右偏身
 	uint16 m,n,p,q,a,b;
 	int i; 
  	m=PWM_Value[0];
  	n=PWM_Value[5]; 
  	p=PWM_Value[7];
 	for(i=50;i>0;i--)        
    	{ 
	 	m=m-1;
	 	n=n-2;
	 	delay();//delay();//delay();delay();
	 	PWM_Value[0]=m;
	 	PWM_Value[5]=n;
    	}
 	for(i=100;i>0;i--)        
    	{ 
	 	p=p+1;
	 	delay();///delay();//delay();delay();
	 	PWM_Value[7]=p;
    	}	
 	for(i=50;i>0;i--)        
    	{ 
	 	m=m+1;
	 	n=n+2;
	 	delay();//delay();//delay();delay();
	 	PWM_Value[0]=m;
	 	PWM_Value[5]=n;
    	}
	for(i=50;i>0;i--)        
    	{ 
	 	m=m+2;
	 	n=n+1;
	 	delay();//delay();//delay();delay();
	 	PWM_Value[0]=m;
	 	PWM_Value[5]=n;
    	}
 	for(i=100;i>0;i--)        
    	{ 
	 	p=p-1;
	 	delay();//delay();//delay();delay();
	 	PWM_Value[7]=p;
    	}	
 	for(i=50;i>0;i--)        
    	{ 
	 	m=m-2;
	 	n=n-1;
	 	delay();//delay();//delay();delay();
	 	PWM_Value[0]=m;
	 	PWM_Value[5]=n;
    	}
}

void main()
{
  	InitPWM(); 
  	LED1=1;
  	LED2=1;
  	LED3=1;
  	LED4=1;
  	delay_nus(80);
  	Movementa();// while(1);
  	delay_nus(20);
  	while(1)
  	{
 		Movementb(); 
 		delay_nus(20);
		jiance1(); 
		jiance3();
 		Movementc();
 		delay_nus(20); 
		jiance();  
		jiance2();
 	}  
}