#include <stc15.h>

typedef unsigned char uint8;   	/*定义可移植的无符号8位整数关键字*/            
typedef signed  char int8;    	/*定义可移植的有符号8位整数关键字*/            
typedef unsigned int uint16;  	/*定义可移植的无符号16位整数关键字*/           
typedef signed  int int16;    	/*定义可移植的有符号16位整数关键字*/           
typedef unsigned char BYTE;
typedef unsigned int	WORD;

#define uchar unsigned char
#define uint unsigned int 
#define STP 4   		// debug step

#define PWM_MAX 3500
#define PWM_MIN 200
#define PWM_MAX_BUF (PWM_MAX-10)
#define PWM_MIN_BUF (PWM_MIN+10)

#define debug  1

//PWM的输出端口

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;

				   
/******************全局变量*************************/       
//控制舵机轮转     
volatile uint16 ad_speed,li_speed; 
unsigned char ReData,SenData;
bit busy,data_full;
int16 u=0;
uint16 code PWM_STD[6]={1430,1240,1530,1430,1220,1530};  //舵机初值
unsigned char order=1;  
/***********************函数声明********************/
void UART_Transimit(unsigned char dat); 
unsigned char UART_Receive(void);                    
void setspeed(uint16,uint16);
void stand_by(int16 (*p)[6],uint8,uint8);
void debug_act(void);   
void Init_System(void);
void delay_ms(unsigned int);								 

uint16  PWM_Value[6]={490,440,1310,1400,2120,1340};   	//舵机实际值

/**********************动作数组*********************/
int16 code stand_walk[ ][6]=   				//向前走五步
{	 
};   

int16 code go_straight[][6]=         			//直走
{
};  


int16 code stand_rf[][6]=           			//前滚翻
{ 
};
     
int16 code stand_rb[][6]=           			//后滚翻
{    
};   
/***********************主函数*********************/

void main(void)
{  
	P2M1=0;
 	P2M0=0;   
	Init_System();
	delay_ms(5000);            			//delay time
	#ifdef debug   

	/*向前走五步*/
	setspeed(280,50);
	stand_by(go_straight,0,61);

	/*前滚翻*/
	setspeed(135,100);
	stand_by(stand_rf,0,54);

	/*向前走五步*/
	setspeed(280,10);	
	stand_by(go_straight,0,61);

	/*后滚翻*/
	setspeed(135,100);
	stand_by(stand_rb,0,54);

	/*直走*/
	setspeed(280,10);				
	stand_by(stand_walk,0,202);
	stand_by(stand_walk,3,202);
	stand_by(stand_walk,3,202);
	stand_by(stand_walk,3,202);
	stand_by(stand_walk,3,211);

		delay_ms(200); 	
	//	debug_act();				//调试函数		  			 

	#endif 
		    
	#ifdef debug             
		delay_ms(50);
		UART_Transimit(' ');
		UART_Transimit('O');
		UART_Transimit('K');
		UART_Transimit('!'); 
		UART_Transimit(0x0d);
		UART_Transimit(0x0a);
	#endif

	 //向前走
	 //set walk speed	
	 //stand_by(stand_walk,0,46);        
		delay_ms(200);                  			
}
/*****************功能函数********************/
void stand_by(int16 (*p)[6],uint8 start,uint8 end)
{ 
	int16 i,j;
	uint8 num=0,num1=0;             		//count ready motors.
	int16 temp[3];
	int16 temp1[3];           			//store the actions steps  
	uint16 delay;            			//for action slow
 
	for(i=start;i<=end;i++)
	{   
	 	
		u=i;    
	    for(j=0;j<3;j++)     			//store action steps
	    {    
			temp[j]=p[i][j]-PWM_Value[j];
			temp1[j]=p[i][j+3]-PWM_Value[j+3];
		}

		while(1)
		{
	      	 num=3;
 		 num1=3;
	         for(j=0;j<3;j++)   			//count each motors step
	        {
	            if(temp[j]==0)
	            {
			num--;	
	            }
	            if(temp[j]>0) 
	            {
	                (PWM_Value[j])++;
	                temp[j]--;
	            }
	            if(temp[j]<0)
	            {
	                PWM_Value[j]--;
	                temp[j]++;
	            }
	            if(temp1[j]==0)
	            {
			num1--;
	            }
		    if(temp1[j]>0) 
	            {
	                (PWM_Value[j+3])++;
	                temp1[j]--;
	            }
	            if(temp1[j]<0)
	            {
	                PWM_Value[j+3]--;
	                temp1[j]++;
	            } 
	        }
	        if(num==0&&num1==0) break;          // if all the motor is ready, break. 
	        for(delay=0;delay<ad_speed;delay++);//delay      
	    }                
      	for(delay=0;delay<li_speed;delay++); 
  }
}  
/**********************调试函数**********************/
void debug_act(void)
{
    int16 temp;
    uint8 i;   
    uint8 char0,char1;
    int16 tempwm;
    while (1)
    {  
        switch(UART_Receive())     			//串口通讯协议
        {
            case 'q': //motor 0 inc
		PWM_Value[0]= PWM_Value[0]<PWM_MIN_BUF? PWM_MIN:PWM_Value[0]-STP;
                break;   
                
            case 'a': //motor 0 dec
		PWM_Value[0]= PWM_Value[0]>PWM_MAX_BUF? PWM_MAX:PWM_Value[0]+STP;
                break;    
                
            case 'w': //motor 1 inc
		PWM_Value[1]= PWM_Value[1]<PWM_MIN_BUF? PWM_MIN:PWM_Value[1]-STP;
                break;    
                
            case 's': //motor 1 dec
		PWM_Value[1]= PWM_Value[1]>PWM_MAX_BUF? PWM_MAX:PWM_Value[1]+STP;
                break;     
                
            case 'e': //motor 2 inc
		PWM_Value[2]= PWM_Value[2]>PWM_MAX_BUF? PWM_MAX:PWM_Value[2]+STP;
                break;   
                
            case 'd': //motor 2 dec
		PWM_Value[2]= PWM_Value[2]<PWM_MIN_BUF? PWM_MIN:PWM_Value[2]-STP;
                break;   
            
	    case 'r': //motor 3 inc
		PWM_Value[3]= PWM_Value[3]>PWM_MAX_BUF? PWM_MAX:PWM_Value[3]+STP;
                break;    
                    
            case 'f': //motor 3 dec
		PWM_Value[3]= PWM_Value[3]<PWM_MIN_BUF? PWM_MIN:PWM_Value[3]-STP;
                break;                  
            
	    case 't': //motor 4 inc
		PWM_Value[4]= PWM_Value[4]<PWM_MIN_BUF? PWM_MIN:PWM_Value[4]-STP;
                break; 

            case 'g': //motor 4 dec
		PWM_Value[4]= PWM_Value[4]>PWM_MAX_BUF? PWM_MAX:PWM_Value[4]+STP;
                break;    
                
            case 'y': //motor 5 inc
		PWM_Value[5]= PWM_Value[5]>PWM_MAX_BUF? PWM_MAX:PWM_Value[5]+STP;
                break;   
        
            case 'h': //motor 5 dec
		PWM_Value[5]= PWM_Value[5]<PWM_MIN_BUF? PWM_MIN:PWM_Value[5]-STP;
                break;

            case 'p':
		UART_Transimit((uint8)(u/10+'0'));
		UART_Transimit((uint8)(u%10+'0')); 
		UART_Transimit(0x0d);
		UART_Transimit(0x0a);    
	     	break;     

            case 'z': //send pwm data 
                UART_Transimit('{'); 
                for(i=0;i<6;i++)
                {
                    temp=PWM_Value[i];
                    if(temp>999) UART_Transimit((uint8)(temp/1000)+'0');  //零消隐
                    else UART_Transimit(' ');
                    temp%=1000;                                 
                    UART_Transimit((uint8)(temp/100)+'0');
                    temp%=100;
                    UART_Transimit((uint8)(temp/10)+'0');
                    temp%=10;
                    UART_Transimit((uint8)temp+'0');
                    if(i!=5) UART_Transimit(',');
                }
                UART_Transimit('}');
                UART_Transimit(',');
                UART_Transimit(0x0d);
                UART_Transimit(0x0a);
                break;
                
            case 'n'://reset
                for(i=0;i<6;i++)
                    PWM_Value[i]=PWM_STD[i];
                break;
            
			/**************设置动作数组**************/    
            case '{':        
                {
                    
                    for(char0=0;char0<6;char0++)
                    {   
                        char1=UART_Receive();
                        if(char1!=' ') tempwm=((int16)(char1-'0'))*1000;
                        else tempwm=0;
                        tempwm+=((int16)(UART_Receive()-'0'))*100;
                        tempwm+=((int16)(UART_Receive()-'0'))*10;
                        tempwm+=(int16)(UART_Receive()-'0');
                        UART_Receive();
                        //if(tempwm>2500 || tempwm<300) continue;
                        PWM_Value[char0]=tempwm;   
                    }   
                }
                break;
                                
            default:
                break;
        } 
    }
}

/***********************************************
function: set speed                             
**********************************************/
void setspeed(uint16 adjacent_spd,uint16 line_spd)
{
    	ad_speed=adjacent_spd;
	li_speed=line_spd;
}

/**********12M晶振1ms延时************/
void delay_ms(unsigned int n)
{
	uint16 i,k;
	for(k=n;k>0;k--)
	for(i=121;i>0;i--);
}

/****************初始化设置*****************/

void Init_System (void)
{								  
    	SCON = 0x50;      		//REN=1允许串行接受状态，串口工作模式1    	       	   
	AUXR &= 0xFB;    		//定时器2时钟为Fosc/12,即12T
	T2L = 0xE6;		    	//设定定时初值
	T2H = 0xFF;		 	//设定定时初值
	AUXR |= 0x01;		 	//串口1选择定时器2为波特率发生器
	AUXR |= 0x10;		 	//启动定时器2
	ES = 1;             		//开串口中断                  
	EA = 1;             		//开总中断 

	TMOD=0x11;
	TH0=-2500/256;
	TL0=-2500%256;
	ET0=1;
	TR0=1;
}
/***************************************************
function: Uart transimit funcition
***************************************************/
void UART_Transimit(unsigned char dat)
{ 
	while(busy);
	busy = 1;
	SBUF = dat;
}   

/***************************************************
function: Uart receive funcition
***************************************************/
unsigned char UART_Receive(void)
{
	while(!data_full);
	data_full = 0;
	UART_Transimit(ReData);
	return ReData;
}

/*========================================================================
    定时器T0的中断服务程序
    一个循环20MS  = 8*2.5ms
========================================================================*/
void timer0(void) interrupt 1 using 3
{	
	switch(order)
	{
		case 1: PWM_OUT0=1;
				TH0=-PWM_Value[0]/256;
				TL0=-PWM_Value[0]%256;
				break;
		case 2: PWM_OUT0=0;       
				TH0=-(3000-PWM_Value[0])/256;
				TL0=-(3000-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=-(3000-PWM_Value[1])/256;
				TL0=-(3000-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=-(3500-PWM_Value[2])/256;
				TL0=-(3500-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=-(3500-PWM_Value[3])/256;
				TL0=-(3500-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=-(3500-PWM_Value[4])/256;
				TL0=-(3500-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;
				order=0;	
				TH0=-(3500-PWM_Value[5])/256;
				TL0=-(3500-PWM_Value[5])%256;
				break;
		default : break;
    }
    	order++;
	TF0=0;
}

/****************************************************
               串口中断程序
******************************************************/
void Uart_Isr (void) interrupt 4 using 1
{
	if(RI)        		  	//RI接受中断标志
	{
	 	RI = 0;	     	  	//清除RI接受中断标志
		ReData = SBUF;  	//SUBF接受/发送缓冲器
		data_full=1;	     	//是否接收到数据
	}
	if(TI)
	{
		TI = 0;
		busy = 0;
	}
}