pid.c

2812的PID.rar

#include "DSP281x_Device.h"     // DSP281x Headerfile Include File
#include "DSP281x_Examples.h"   // DSP281x Examples Include File
#include "ctr.h"
#include "PID.h"
#include "scancode.h"

// Prototype statements for functions found within this file.
interrupt void cpu_timer0_isr(void);

#define LEDS *(int *)0xc0000
#define port8000 (*((unsigned int *)0x108000))
#define port8001 (*((unsigned int *)0x108001))
#define port8002 (*((unsigned int *)0x108002))
#define port8003 (*((unsigned int *)0x108003))
#define port8004 (*((unsigned int *)0x108004))
#define port8007 (*((unsigned int *)0x108007))

void Gpio_select(void);
void Gpio_PortB(void);
void ShowParameters();
void LCDPutString(unsigned int *pData,int x,int y,unsigned int nCharNumber,unsigned color);
void PIDControl(int rk,int yk);
char ConvertScanToChar(unsigned char cScanCode);

Uint16 var1 = 0;
Uint16 var2 = 0;
Uint16 var3 = 0;
Uint16 test_count = 0;
Uint16 Test_flag = 0;
Uint16 Test_var  = 0;
Uint16 Test_status[32];

int i=0,nnn;
unsigned int nCount,nCount1,nCount2,nCount3,nJSSpeed,uWork,uN;
unsigned int uLBD;
float a=0.6f,b=0.2f,c=0.1f,duk;
int ek,ek1,ek2,tz;
int nInput;
int nSSS,pwm;
int md,wc;
unsigned int nScreenBuffer[1024];

void main(void)
{   
	
	unsigned int uPort8000;
	unsigned int nScanCode;
	unsigned char cKey;
	int speed[100],sp,lj;
	float ljh;
	char strInput[4];
	int i,w1,w2,w3;

	uLBD=nnn=0;
	nCount=nCount1=nCount2=nCount3=nJSSpeed=0;
	for ( sp=0;sp<100;sp++ )	speed[sp]=0;
	sp=nSSS=nCount=nCount1=nCount2=nJSSpeed=0; cKey=0; nInput=tz=wc=0;
	ek=ek1=ek2=0;
	for ( uWork=0;uWork<4;uWork++ )	strInput[uWork]=0;
	uN=100; md=70; pwm=0;
// Step 1. Initialize System Control:
// PLL, WatchDog, enable Peripheral Clocks
// This example function is found in the DSP281x_SysCtrl.c file.
   InitSysCtrl();

// Step 2. Initalize GPIO: 
// This example function is found in the DSP281x_Gpio.c file and
// illustrates how to set the GPIO to it's default state.
// InitGpio();  // Skipped for this example  


// Step 3. Clear all interrupts and initialize PIE vector table:
// Disable CPU interrupts 
   DINT;

// Initialize the PIE control registers to their default state.
// The default state is all PIE interrupts disabled and flags
// are cleared.  
// This function is found in the DSP281x_PieCtrl.c file.
   InitPieCtrl();
   
// Disable CPU interrupts and clear all CPU interrupt flags:
   IER = 0x0000;
   IFR = 0x0000;

// Initialize the PIE vector table with pointers to the shell Interrupt 
// Service Routines (ISR).  
// This will populate the entire table, even if the interrupt
// is not used in this example.  This is useful for debug purposes.
// The shell ISR routines are found in DSP281x_DefaultIsr.c.
// This function is found in DSP281x_PieVect.c.
   InitPieVectTable(); 

// Interrupts that are used in this example are re-mapped to
// ISR functions found within this file.  
   EALLOW;  // This is needed to write to EALLOW protected registers
   PieVectTable.TINT0 = &cpu_timer0_isr;
   EDIS;    // This is needed to disable write to EALLOW protected registers

// Step 4. Initialize all the Device Peripherals:
// This function is found in DSP281x_InitPeripherals.c
// InitPeripherals(); // Not required for this example

   /////InitCpuTimers();   // For this example, only initialize the Cpu Timers
   	CpuTimer0.RegsAddr = &CpuTimer0Regs;
	// Initialize timer period to maximum:	
	CpuTimer0Regs.PRD.all  = 3709*2;
	// Initialize pre-scale counter to divide by 1 (SYSCLKOUT):	
	CpuTimer0Regs.TPR.all  = 0;
	CpuTimer0Regs.TIM.all  = 0;
	CpuTimer0Regs.TPRH.all = 0;
	// Make sure timer is stopped:
	CpuTimer0Regs.TCR.bit.TSS = 1;
	CpuTimer0Regs.TCR.bit.SOFT = 1;
	CpuTimer0Regs.TCR.bit.FREE = 1;
	// Reload all counter register with period value:
	CpuTimer0Regs.TCR.bit.TRB = 1;
	CpuTimer0Regs.TCR.bit.TIE = 1;
	// Reset interrupt counters:
	CpuTimer0.InterruptCount = 0;	         


   StartCpuTimer0();

// Step 5. User specific code, enable interrupts:


// Enable CPU INT1 which is connected to CPU-Timer 0:
   IER |= M_INT1;

// Enable TINT0 in the PIE: Group 1 interrupt 7
   PieCtrlRegs.PIEIER1.bit.INTx7 = 1;

// Enable global Interrupts and higher priority real-time debug events:
   EINT;   // Enable Global interrupt INTM
   ERTM;   // Enable Global realtime interrupt DBGM
   Gpio_PortB();
   *(int *)0x108000=0;		// 初始化ICETEK-CTR
   *(int *)0x108000=0x80;
   *(int *)0x108000=0;
   *(int *)0x108007=0;			// 关闭东西方向的交通灯
   *(int *)0x108007=0x40;	// 关闭南北方向的交通灯
  uPort8000=*(int *)0x108002;
	// 设置显示参数和内容
	LCDSetDelay(1);		// 设置延时等待参数
	LCDSetScreenBuffer(nScreenBuffer);	// 显示缓冲区
	LCDTurnOn();			// 打开显示
	LCDCLS();				// 清除显示内存   
	LCDPutCString(str1,0,63,8,0);
	LCDPutCString(str2,0,47,2,1);
	LCDPutCString(str3,68,47,2,1);
	LCDPutCString(str6,0,31,2,1);
	LCDPutCString(str5,68,31,2,1);
	LCDPutCString(str7,0,15,3,1);
	LCDPutCString(str4,68,15,2,1);
	ShowParameters();		// 参数显示

   *(int *)0x108007=0x0c4;
   
     while ( 1 )
	{
		if ( nCount==0 )	// 读取键盘标志
		{
			nScanCode=port8001;	// 读扫描码
			nScanCode&=0x0ff;	// 低8位
			if ( nScanCode!=0 )
			{
				uWork=port8002;		// 清除键盘缓冲区
				if ( nScanCode==SCANCODE_Num )	break;	// NUM键退出
				else
				{
					if ( nScanCode==SCANCODE_Enter )	// 按回车键输入速度
					{
						uWork=strInput[0]*10+strInput[1];	// 计算调整速度
						md=uWork;
						for ( uWork=0;uWork<2;uWork++ )	strInput[uWork]=0;
						nInput=0;
						LCDPutString(numbers,104,15,1,1);
						LCDPutString(numbers,112,15,1,1);
						LCDPutString(numbers,120,15,1,1);
						LCDRefreshScreen();
					}
					else
					{
						cKey=ConvertScanToChar(nScanCode);	
						if ( cKey>='0' && cKey<='9' )		// 输入速度值
						{
							strInput[nInput]=cKey-'0';
							nInput++; if ( nInput>=2 )	nInput=0;
							uWork=strInput[0]*10+strInput[1];	// 计算调整速度
							w1=uWork%1000/100; w2=uWork%100/10; w3=uWork%10;
							LCDPutString(numbers+w1*8,104,15,1,1);
							LCDPutString(numbers+w2*8,112,15,1,1);
							LCDPutString(numbers+w3*8,120,15,1,1);
							LCDRefreshScreen();
						}
					}
				}
			}
		}
		if ( nJSSpeed==0 )	// 读取速度标志
		{
			nJSSpeed=0;
			nSSS=port8003;	// 从端口读取速度计数
			nSSS&=0x0ff;
			if ( nSSS>=0 && nSSS<100 )	// 合法性检测
			{
				speed[sp]=nSSS; 		// 读取66个计数值
				sp++; sp%=66;
			}
			if ( sp==0 )	// 是否已经读了66个速度？
			{				// 以下求速度平均值
				lj=0; ljh=0;
				for ( i=50;i<66;i++ )
				{
					if ( speed[i]>=0 && speed[i]<150 )
					{
						ljh+=speed[i];
						lj++;
					}
				}
				wc=( lj==0 )?(0):(ljh/lj);
				if ( wc>150 )
				{	
					wc=0;
				}
				nCount3++; nCount3%=3;
				if ( nCount3==2 )		
				{
					PIDControl(md,wc);	// 调用PID算法控制程序进行控制
					uN=100-pwm;			// 利用占空比调整控制
					ShowParameters();	// 显示各参数值到LCD
				}
			}
		}
	}

} 

// PID算法控制子程序-------------------------------------------------------------------------
void PIDControl(int rk,int yk)
{
	ek=rk-yk;
	duk=a*ek+b*ek1+c*ek2;		// 计算控制输出
	ek2=ek1; ek1=ek;
	if ( duk>10 )	duk=10;		// 幅度限制
	tz=(int)duk;
	pwm+=tz;					// 计算当前占空比
	if ( pwm<0 )	pwm=0;
	else if ( pwm>99 )	pwm=99;
}

#define Hz200 100
#define Hz1 20000
interrupt void cpu_timer0_isr(void)
{
   PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
   CpuTimer0Regs.TCR.bit.TIF = 1;
   CpuTimer0Regs.TCR.bit.TRB = 1;
	nCount1++; nCount1%=Hz200;
	GpioDataRegs.GPBDAT.bit.GPIOB4=( nCount1>uN )?1:0;
   if ( nCount2==0 )
   {
		GpioDataRegs.GPBDAT.bit.GPIOB5=nnn;
		nnn=1-nnn;
   }
   nCount2++; nCount2%=Hz1;
	nJSSpeed++; nJSSpeed%=12000;	// 读取速度标志
	nCount++; nCount%=51200;
}


void Gpio_select(void)
{
     EALLOW;
     
    GpioMuxRegs.GPAMUX.all=var1;   // Configure MUXs as digital I/Os or
    GpioMuxRegs.GPBMUX.all=var1;   // peripheral I/Os
    GpioMuxRegs.GPDMUX.all=var1;
    GpioMuxRegs.GPFMUX.all=var1;         
    GpioMuxRegs.GPEMUX.all=var1; 
    GpioMuxRegs.GPGMUX.all=var1;
                                        
    GpioMuxRegs.GPADIR.all=var2;   // GPIO PORTs  as output
    GpioMuxRegs.GPBDIR.all=var2;   // GPIO DIR select GPIOs as output 
    GpioMuxRegs.GPDDIR.all=var2;
    GpioMuxRegs.GPEDIR.all=var2;        
    GpioMuxRegs.GPFDIR.all=var2; 
    GpioMuxRegs.GPGDIR.all=var2;

    GpioMuxRegs.GPAQUAL.all=var3;  // Set GPIO input qualifier values
    GpioMuxRegs.GPBQUAL.all=var3;   
    GpioMuxRegs.GPDQUAL.all=var3;
    GpioMuxRegs.GPEQUAL.all=var3;
 
    EDIS;
     
}     

void Gpio_PortB(void)
{    

// GPIO Test #2: 
// Configure Upper 8 bits of Port as inputs and lower 8 bits as outputs    
// Loop back bits [7:0] to bits [15:8]
// Don't set any input qualifier
 
    var1= 0x0000;       // sets GPIO Muxs as I/Os
    var2= 0x00FF;       // sets GPIO 15-8 DIR as inputs, 7-0 DIR as outputs
    var3= 0x0000;       // Don't set any input qualifier
    
    Gpio_select();    
    
    test_count = 0;
    Test_status[Test_var] = 0x0002;
    Test_var++;
    
    Test_status[Test_var] = 0xD0BE;      // Set the default value of status 
                                         // to "PASSED"
    GpioDataRegs.GPBCLEAR.all = 0x00FF;  // Test Clear
    asm(" RPT #5 ||NOP");                             
    GpioDataRegs.GPBSET.bit.GPIOB5=1;
    GpioDataRegs.GPBSET.bit.GPIOB4=1;
 
}

void LCDPutString(unsigned int *pData,int x,int y,unsigned int nCharNumber,unsigned color)
{
	int i,j,l;
	unsigned int k,mcolor;
	
	for ( l=0;l<nCharNumber;l++ )
		for ( i=0;i<8;i++ )
		{
			k=1;
			for ( j=0;j<16;j++,k<<=1 )
			{
				if ( color==2 )	mcolor=2;
				else
				{
					mcolor=( pData[l*8+i]&k )?(1):(0);
					if ( color==0 )	mcolor=1-mcolor;
				}
				LCDPutPixel(x+l*8+i,y-j,mcolor);
			}
		}
}

// 显示参数到LCD
void ShowParameters()
{
	int w1,w2,w3;
	
	w1=md%1000/100; w2=md%100/10; w3=md%10;
	LCDPutString(numbers+w1*8,36,47,1,1);
	LCDPutString(numbers+w2*8,44,47,1,1);
	LCDPutString(numbers+w3*8,52,47,1,1);
	w1=wc%1000/100; w2=wc%100/10; w3=wc%10;
	LCDPutString(numbers+w1*8,104,47,1,1);
	LCDPutString(numbers+w2*8,112,47,1,1);
	LCDPutString(numbers+w3*8,120,47,1,1);
	if ( ek>=0 )
	{
		LCDPutString(numbers+88,36,31,1,1);
		w3=((int)ek)%100;
	}
	else
	{
		LCDPutString(numbers+96,36,31,1,1);
		w3=((int)(-ek))%100;
	}
	w1=w3%100/10; w2=w3%10;
	LCDPutString(numbers+w1*8,44,31,1,1);
	LCDPutString(numbers+w2*8,52,31,1,1);
	if ( tz>=0 )
	{
		LCDPutString(numbers+88,104,31,1,1);
		w3=tz;
	}
	else
	{
		LCDPutString(numbers+96,104,31,1,1);
		w3=(-tz);
	}
	w1=w3%100/10; w2=w3%10;
	LCDPutString(numbers+w1*8,112,31,1,1);
	LCDPutString(numbers+w2*8,120,31,1,1);
	w1=pwm%100/10; w2=pwm%10;
	LCDPutString(numbers+w1*8,44,15,1,1);
	LCDPutString(numbers+w2*8,52,15,1,1);
	LCDPutString(numbers+80,60,15,1,1);
	LCDRefreshScreen();
}

char ConvertScanToChar(unsigned char cScanCode)
{
	char cReturn;
	
	cReturn=0;
	// 根据扫描码得到相应字符
	switch ( cScanCode )
	{
		case SCANCODE_0: cReturn='0'; break;
		case SCANCODE_1: cReturn='1'; break;
		case SCANCODE_2: cReturn='2'; break;
		case SCANCODE_3: cReturn='3'; break;
		case SCANCODE_4: cReturn='4'; break;
		case SCANCODE_5: cReturn='5'; break;
		case SCANCODE_6: cReturn='6'; break;
		case SCANCODE_7: cReturn='7'; break;
		case SCANCODE_8: cReturn='8'; break;
		case SCANCODE_9: cReturn='9'; break;
		case SCANCODE_Plus: cReturn='+'; break;
		case SCANCODE_Minus: cReturn='-'; break;
	}
	 
	return cReturn;
}
// No more.
//========================================