temp.c

使用TI DSP 2407A 进行开发的源代码

#include "DSP240x_Device.h"




double Kp=5,Ki=0.05,Kd=4;
double u_1=0,u=0;
unsigned int y_1=0,yy=830;
int err_1=0,err=0,ed=0,ei=0,direct=1,dir_1=1,dir_2=1;

void invert_PID()
{

   err = yy - 822;
   ed = err - err_1;
   ei = ei + err;
   
  
   u=Kp*err+Kd*ed+Ki*ei;
   
   if(u>0)
      direct = 2;
   else
      direct = 1;   
      
   u=abs(u);
   
   if(u<25)
      u=0;
      
    if(u>20)
      u=u+280;
   
   if(u>950)
      u=950;
       
   
   err_1 = err;  
   y_1 = yy; 
   u_1 = u;
   dir_2 = dir_1;
   dir_1 = direct;   

}




main()
{

      unsigned int light_val[2];
        
        unsigned int temp1,temp2;
        
	sysinit();
/*
	IO_Init();
	test();
	
	init_ev();
	
    EV_TEST();
	AD_Init();

	AD_Test();

   Init_Sci();
   
   SCI_TEST();
   
  */ 
   
      init_ev();
   
      AD_Init();
   
      Drive_Init();
      
     // IO_Init();


  // AD_Test();
	
  //  EV_TEST();


	while(1)
	{
		//if(nNewConvert)
//	if(0)
		{
			//nNewConvert=0;
			
		//		printf("%d",light_val[0]);
			
		//	temp1 = IN_Status_Load(CAP1_DATDIR,CAP1_HIGH);
		//	temp2 = IN_Status_Load(CAP2_DATDIR,CAP2_HIGH);
			
			
	//		Drive(LEFT, FORWARD, light_val[0]);
	//		Drive(RIGHT, FORWARD, light_val[1]);
			
			
			//    #define    LEFT      1
//    #define    RIGHT    2



//   #define    FORWARD      1
//   #define    BACK    2

             
           

			light_value(light_val,15);
			
			yy = light_val[0];
			
			invert_PID();
			
		/*	if(dir_2 !=dir_1)
			   {
			   
			   	Drive(LEFT,direct,900);			
			    Drive(RIGHT,direct,900);			    
			    wait_ms(20);
			    
			    }
			  */  
			 
		  temp1 = (u*10)/990;  
		  temp2= 12 - temp1; 
			
			
	    	Drive(LEFT,direct,940);
			
	    	Drive(RIGHT,direct,990);
	    	
	    	wait_ms(temp1);
	    	
	    	Drive(LEFT,direct,0);
			
	    	Drive(RIGHT,direct,0);
	    	
	    	wait_ms(temp2);
	    	
	    	

	    	
			
		
			
		
		
			
		
	//	Drive(RIGHT,BACK,300);
			
			
		//	Drive(RIGHT, BACK, 700);
			
	/*		
			if(temp1 ==0)
			{
		//	Drive(LEFT, BACK, light_val[0]);
		
			Drive(LEFT, BACK, 300);
			}
			
			if(temp2 ==0)
			{
			
		//	Drive(RIGHT, BACK, light_val[1]);
			
				Drive(RIGHT, BACK, 700);
			
			}
			
			if((temp1==0)&&(temp2==0))
			{
			
		//	Drive(LEFT, BACK, light_val[1]);
		//	Drive(RIGHT, BACK, light_val[0]);
			
			Drive(LEFT, BACK, 700);
			Drive(RIGHT, BACK, 300);
			
			}
			
		*/	
			
		
			
			//Sci_Add_String(16,res,uart_rec_char);
	    		
		}	
		
		
	//     Sci_Get_String(uart_rec_char);
	    // Sci_Put_String ( uart_rec_char ) ;
	}
		
}