driver.c

LPC2129实现从SPI通讯和两个串口定时器中断

   
    temp=com_pk->data[1];
	temp<<=8;
	temp|=com_pk->data[2];
    temp2=com_pk->data[0];
	if(temp2==1 || temp2==2)
	{
       
		 if(temp2==1)
		 UART1_CS(2); 
		 if(temp2==2)
		 UART1_CS(1); 
		 cha_send(cha,6);
		 data_send(temp);
		 U1THR=0x0D;
		 while( (U1LSR&0x40)==0 );  
		 EEPSAV(); 
		 send_same_back_uart(0,com_pk);
	}
	else
	{ 
	     if(temp2==3)
		 UART1_CS(2); 
		 if(temp2==4)
		 UART1_CS(1); 

		 G_ENCRES();
  	 }

}
/****************************************************************************
* 名称：void G_ENCRES(void)
* 功能：查询编码器分辨率
* 入口参数: 无    
* 出口参数：无
***************************************************************************/
void G_ENCRES(void)
{  
  uint8 cha[8]="GENCRES";
  uint8 temp3,data,en_index=0,index=0,en[6];
  uint16   endata;
	 cha_send(cha,7);
	 U1THR=0x0D;
	 while( (U1LSR&0x40)==0 ); 
	 Delay(50);
	 while((U1LSR&(1<<0) == (1<<0)))  
	{   
		en[en_index++]=U1RBR;		
	}
	for(index=0;index<2;index++)
	{
		TXD_BUF[0].data[index]= 0;	
	}

	for(index=0;index<6;index++)
	{
			temp3=en[index];
			if((temp3!=0x0D)&&(temp3!=0x0A))
			{  		
				data=temp3-0x30;
				endata*=10;
				endata+=data;
				
			}	
			else
			 	break;
	}

	TXD_BUF[0].data[0]=(endata>>8)&0x000000FF;
    TXD_BUF[0].data[1]=endata&0x000000FF;
	TXD_BUF[0].IP_addr=0x01;
	TXD_BUF[0].length=0x02;
	TXD_BUF[0].command=0x2F;
	set_sum(&TXD_BUF[0]);  
  	TXD_BUF[0].state=0x00;
		
	 while( ((TXD_BUF[0].state)&SET_NEW)!=0x01 )
	 {	 
		 while((U0LSR&0x40)==0);
		 uart_tx_isr(0);	
	 }		 
	}
/****************************************************************************
* 名称：void dr_init(void)
* 功能：驱动器参数初始化
* 入口参数: 无    
* 出口参数：无
***************************************************************************/
void dr_init(void)
{
  UART1_CS(4);
 // set_ENCRES(48);
  EEPSAV();
  AC_set(1000);
  EEPSAV();
  set_SP(6000);
  EEPSAV();
  PID_set('p',100);	 
  EEPSAV();
  PID_set('i',10);
  EEPSAV();
  PID_set('d',3);
  EEPSAV();
}
 /****************************************************************************
* 名称：void sf(void)
* 功能：设置锁定航向函数各参数初始值
* 入口参数: 无    
* 出口参数：无
***************************************************************************/ 
void set_sf(struct DATA_BUF *com_pk)
{
	int angle;
    uint8 I2c_data[5];
	int tuo_data;
	IO0SET|=0x00400000;			  //开485总线

    IRcvByte(0x50,I2c_data);
	Delay(1);
	tuo_data=(int)I2c_data[0]<<8;		
    IRcvByte(0x50,I2c_data);			
	tuo_data|=I2c_data[0];

	IRcvByte(0x50,I2c_data);
	angle1=I2c_data[0];
	IRcvByte(0x50,I2c_data);
    angle2=I2c_data[0];

	angle=(((com_pk->data[0])&0x7f)<<8)+com_pk->data[1];  //读取上位机设定的方向值
	if((com_pk->data[0]&0X80)==0x80)	 //右转,
       sf=angle+tuo_data;
	else
	   sf=-angle+tuo_data;		 //自此以上为计算锁定值

//	  sf=((com_pk->data[0]&0x7f)<<8)+com_pk->data[1]; 
	/* sf=com_pk->data[0]<<8;	        //直接设定锁定值
	   sf|= com_pk->data[1];
	*/
	
    speed=(((com_pk->data[2])&0x7f)<<8)+(com_pk->data[3]);//读取运动速度

	 Kp=(com_pk->data[4]);	 //计算PID参数
     Ti=(com_pk->data[5]);
	 Td=(com_pk->data[6]);
	 A=(int16)(Kp+Kp/10/Ti+Kp*Td*10);
	 B=(int16)(Kp+2*Kp*Td*10);
	 C=(int16)(Kp*Td*10);
	 
	 Er=com_pk->data[7];
	 Ur=(com_pk->data[8]<<8)+com_pk->data[9];

/*	 U0THR=sf>>8;	   //告知上位机锁定的航向
  	 Delay(4);
	 U0THR=sf;
	 Delay(4);		 */

    	TXD_BUF[0].IP_addr = 0x01;							//设置发送从机的IP地址
		TXD_BUF[0].command = com_pk->command;					//设置反回发送命令，
		TXD_BUF[0].length = 2;

		TXD_BUF[0].data[0] = sf>>8;		
		TXD_BUF[0].data[1] = sf;		

		set_sum (&TXD_BUF[0]);							//设置校验和	
		uart_tx_isr(0);								//开始发送数据
        IO0CLR|=0x00400000;	 //关485总线		 
    


}
/****************************************************************************
* 名称：void sdhx(struct DATA_BUF *com_pk)
* 功能：锁定航向函数
* 入口参数: 无    
* 出口参数：无
***************************************************************************/ 
 void sdhx(struct DATA_BUF *com_pk)
{	
	uint8 I2c_data[2];
	int tuo_data;
	
	IO0SET|=0x00400000;			  //开485总线
	
	 
    	IRcvByte(0x50,I2c_data);
		Delay(1);
	    tuo_data=I2c_data[0]<<8;
	
		IRcvByte(0x50,I2c_data);
	    tuo_data|=I2c_data[0];

		IRcvByte(0x50,I2c_data);
		angle1=I2c_data[0];
		IRcvByte(0x50,I2c_data);
		angle2=I2c_data[0];
				   //以上为不停的扫描当前方向值（陀螺仪输出值），
		                           //以便和锁定的航向值作比较

  		U_k=0;					   //pid调节量清0
		ek=sf-tuo_data;			   //偏差
		
		if(ek>Er||ek<(-Er))		   //设置死区大小
  		{	
			
	 		U_k=U_kk+A*ek-B*ekk+C*ekkk;	  
			                  /*此式为pid数字控制器编程形式，在此用试验法试出
			                  ek的系数，为调试方便ekk,ekkk前的系数暂作0处理。有兴趣的读者
							  可用阶跃曲线法或其他方法测出此处的数学模型，计算出pid参数。采样
							  频率为1.92KHZ。	*/
  		}		
		    U_kk=U_k;
    		ekkk=ekk;
			ekk=ek;
	   
	    if(U_k>Ur)		   //限制最大输出量，
			U_k=Ur;
		if(U_k<(-Ur))
			U_k=(-Ur);	
		else
	     	;	
		if((com_pk->data[2]&0x80)==0)      		
	
			 SendSpeedLR(speed+U_k,speed-U_k);      	//前进    
		else 	    
	         SendSpeedLR(-speed+U_k,-speed-U_k);		//后退

			
/*	  		U0THR=tuo_data>>8;	   //告知上位机当前的航向
			Delay(4);
			U0THR=tuo_data;
			Delay(4);
  */

		if(inn==1)
		{ Led_off();                    
	  	  inn=0;
	    }
		else
		{ Led_on(); 
		  inn=1;
		}	
    
		TXD_BUF[0].IP_addr = 0x01;							//设置发送从机的IP地址
		TXD_BUF[0].command = com_pk->command;					//设置反回发送命令，
		TXD_BUF[0].length = 2;

		TXD_BUF[0].data[0] = tuo_data>>8;		
		TXD_BUF[0].data[1] = tuo_data;		
	//	TXD_BUF[0].data[2] = angle1;
    //	TXD_BUF[0].data[3] = angle2;


		set_sum (&TXD_BUF[0]);							//设置校验和	
		uart_tx_isr(0);								//开始发送数据
  
        Delay(10);
        tuo_data=0;	  

		 
   IO0CLR|=0x00400000;	 //关485总线		 
     

}    
/****************************************************************************
* 名称：void turn_angle(uint16 angle)
* 功能：上位机控制旋转任意角度
* 入口参数:   
* 出口参数：无
***************************************************************************/
void turn_angle(struct DATA_BUF *com_pk)
{	uint8 I2c_data[5];
	int tuo_data,tuo_set;
	int angle;
	int speed;
	angle=(((com_pk->data[0])&0x7f)<<8)+com_pk->data[1];  //读取上位机设定的方向值
	
	speed=(((com_pk->data[2])&0x7f)<<8)+com_pk->data[3]; //读取上位机设定的速度值
	
//	angle = angle*689;  //7800;
    IO0SET|=0x00400000;	    
	
    IRcvByte(0x50,I2c_data); //读取陀螺仪当前值
	Delay(1);
	tuo_data=I2c_data[0]<<8;		
    IRcvByte(0x50,I2c_data);
	tuo_data|=I2c_data[0];
	
	IRcvByte(0x50,I2c_data);
	;
    IRcvByte(0x50,I2c_data);   


										        

		if((com_pk->data[0]&0X80)==0x80)	 //右转
     	{	
			tuo_set=angle+tuo_data-0x19; 		// 0x1c为程序执行周期产生的误差作修正 地面移动为0x05
			Delay(1);
		
		    SendSpeedLR(speed,-speed);//右转 
			Delay(1);
			SendSpeedLR(speed,-speed);//右转 
		   	Delay(1);
			SendSpeedLR(speed,-speed);//右转

			while(tuo_data<=tuo_set)		
            {       
	    	IRcvByte(0x50,I2c_data);   //不断读取陀螺仪数据作比较
		//	Delay(1);
	     	tuo_data=I2c_data[0]<<8;		
		   	IRcvByte(0x50,I2c_data);
		    tuo_data|=I2c_data[0];
	
		   	IRcvByte(0x50,I2c_data);
			;
			IRcvByte(0x50,I2c_data);
		  
			if((com_pk->data[2]&0x7f)==0x00)	 //停止指令,速度为0
			  break; 		  
            }
		
		}

	   else    //左转 
   		{  
		 //tuo_set=angle-0x8000;
		   tuo_set=tuo_data-angle+0x19;
	     //tuo_set+=0x19;   	// 0x1c为程序执行周期产生的误差作修正	 
		   Delay(1);
   	   	   SendSpeedLR(-speed,speed);		//左转
		   Delay(1);	
		   SendSpeedLR(-speed,speed);		//左转
		   Delay(1);					   
		   SendSpeedLR(-speed,speed);		//左转

		   while(tuo_data>=tuo_set)		
           {       
	        IRcvByte(0x50,I2c_data);   //不断读取陀螺仪数据作比较
		//	Delay(1);
	     	tuo_data=I2c_data[0]<<8;		
		   	IRcvByte(0x50,I2c_data);
		    tuo_data|=I2c_data[0];
	
		   	IRcvByte(0x50,I2c_data);
			;
			IRcvByte(0x50,I2c_data);
		  
			if((com_pk->data[2]&0x7f)==0x00)	 //停止指令,速度为0
			  break; 		  
			}
	
	   }	
		SendSpeedLR(0,0);	   //stop
			Delay(1);
		SendSpeedLR(0,0);
			Delay(1);
		SendSpeedLR(0,0);
		IO0CLR|=0x00400000;	
}
/****************************************************************************
* 名称：void sdhx2(void)
* 功能：锁定航向函数,为了兼容以前的地面移动机器人上位机软件
* 入口参数: 无    
* 出口参数：无
***************************************************************************/ 
 void sdhx2(struct DATA_BUF *com_pk)
{	

	uint8 I2c_data[5],temp;
	int tuo_data;
	int sf;
	int speed=(((com_pk->data[2])&0x7f)*256)+(com_pk->data[3]);
	
	int U_k=0,U_kk=0,ek;//ekk=0,ekkk=0;
		
//	UART1_CS(4);
	IO0SET|=0x00400000;
	

	ISendByte(0x50,0x1F);		//发送标定指令
	Delay(100);
	IRcvByte(0x50,I2c_data);  
    Delay(10);
	gyro1=I2c_data[0];
	sf=I2c_data[0]<<8;
		
	IRcvByte(0x50,I2c_data);
	Delay(10); 
	gyro2=I2c_data[0];
	sf|=I2c_data[0];
		
	IRcvByte(0x50,I2c_data);
	Delay(10);
	angle1=I2c_data[0];
	IRcvByte(0x50,I2c_data);
	Delay(10);
	angle1=I2c_data[0];


	while(speed)		
  	{                   
		IRcvByte(0x50,I2c_data);
		Delay(4);
		gyro1=I2c_data[0];
        tuo_data=I2c_data[0]<<8;
		IRcvByte(0x50,I2c_data);
		Delay(4);
		gyro2=I2c_data[0];
        tuo_data|=I2c_data[0];
		IRcvByte(0x50,I2c_data);
		Delay(4);
		angle1=I2c_data[0];		
		IRcvByte(0x50,I2c_data);
		Delay(4);
		angle2=I2c_data[0];		

  		U_k=0;
		ek=sf-tuo_data;
		
		if(ek>2||ek<(-2))
  		{
	 		U_k=U_kk+ek/2;    //-ekk/4+ekkk/16;  
  		}		
		U_kk=U_k;
    	//	ekkk=ekk;
		//	ekk=ek;
	   
	    if(U_k>300)
			U_k=300;
		if(U_k<(-300))
			U_k=(-300);	
		if((com_pk->data[2]&0x80)==0)		
//			 SendSpeedLR(-speed+U_k,-speed-U_k);	//前进 
			 SendSpeedLR(speed+U_k,speed-U_k);	//前进    
		else 	    
//			 SendSpeedLR(speed+U_k,speed-U_k);		//后退
			 SendSpeedLR(-speed+U_k,-speed-U_k);		//后退

		//	U0THR=tuo_data>>8;	   //监测数据段
		//	Delay(2);
		//	U0THR=tuo_data;
		//U0THR=ek;
		//Delay(2);				   
		speed=(((com_pk->data[2])&0x7f)*256)+(com_pk->data[3]);	   	  
		tuo_data=0;	

		temp=(RXD_BUF[0].command);
		if(temp==0x21||temp==0x24||temp==0x25||temp==0x26)
		{	
		speed=0;
		}
				 
  	}
	SendSpeedLR(0,0);
	SendSpeedLR(0,0);
    IO0CLR|=0x00400000;			 
   
  
}   	 
/****************************************************************************
* 名称：void turn_angle2(uint16 angle)
* 功能：上位机控制旋转任意角度;为兼容以前的地面移动机器人上位机软件
* 入口参数:   
* 出口参数：无
***************************************************************************/
void turn_angle2(struct DATA_BUF *com_pk)
{	uint8 I2c_data[5];
	int tuo_data=0x8000,tuo_set;
	int speed=(((com_pk->data[2])&0x7f)*256)+(com_pk->data[3]);
	int angle=(((com_pk->data[0])&0x7f)*256)+(com_pk->data[1]);
	angle = angle*689/7800;
//	IsSearch=1;
    IO0SET|=0x00400000;
	UART1_CS(4);

		if((com_pk->data[0]&0X80)==0x80)	 //右转
     	{ tuo_set=angle*2+0x8000-0x05; 		// 0x05可作微调系数
		  ISendByte(0x50,0x1F);		//发送标定指令
 		  Delay(10);				//等待陀螺仪标定完	 
	      SendSpeedLR(speed,-speed);//右转 
		
			while(tuo_data<=tuo_set)		
            {       
	    	IRcvByte(0x50,I2c_data);
			gyro1=I2c_data[0];
	     	tuo_data=(int)I2c_data[0]<<8;		
		   	IRcvByte(0x50,I2c_data);
			gyro2=I2c_data[0];					
		    tuo_data|=I2c_data[0];
	
			
		   	IRcvByte(0x50,I2c_data);
			angle1=I2c_data[0];
			IRcvByte(0x50,I2c_data);
			angle2=I2c_data[0];
	
												
	    	angle=(((com_pk->data[0])&0x7f)*256)+(com_pk->data[1]);
		    speed=(((com_pk->data[2])&0x7f)*256)+(com_pk->data[3]);
		    SendSpeedLR(speed,-speed);//右转 
			SendSpeedLR(speed,-speed);//右转 
			if((com_pk->data[0]&0x80)==0x00||angle==0x00)
			  break; 		  
            }
	 
		}
		SendSpeedLR(0,0);
			Delay(10);
		SendSpeedLR(0,0);
			Delay(10);
		SendSpeedLR(0,0);
	
		if(((com_pk->data[0])&0x80)==0x00)  //左转 
   		{  tuo_set=-angle*2+0x8000+0x05;   	// 
   	   	   ISendByte(0x50,0x1F);		//发送标定指令
 		   Delay(10);					//等待陀螺仪标定完
		  
		   SendSpeedLR(-speed,speed);		//左转	
				  
		   while(tuo_data>=tuo_set)		
           {       
	    	IRcvByte(0x50,I2c_data);
			gyro1=I2c_data[0];
	     	tuo_data=(int)I2c_data[0]<<8;		
		   	IRcvByte(0x50,I2c_data);
			gyro2=I2c_data[0];		
		    tuo_data|=I2c_data[0];
	
			
		   	IRcvByte(0x50,I2c_data);
			angle1=I2c_data[0];
			IRcvByte(0x50,I2c_data);
			angle2=I2c_data[0];


			angle=(((com_pk->data[0])&0x7f)*256)+(com_pk->data[1]);
		    speed=(((com_pk->data[2])&0x7f)*256)+(com_pk->data[3]);
		    SendSpeedLR(-speed,speed);//左转 
			SendSpeedLR(-speed,speed);//左转 
	   		if((com_pk->data[0]&0X80)==0x80||angle==0x00)
			  break; 
									 
         
		   }
	
	   }	
		SendSpeedLR(0,0);
			Delay(10);
		SendSpeedLR(0,0);
			Delay(10);
		SendSpeedLR(0,0);
//		IsSearch=0;
		IO0CLR|=0x00400000;	
}