基于PID算法的电机控制设计(DSP)

#include "scancode.h"
#include "ctr.h"
#include "PID.h"
#include

//定时器分频参数 ------------------------------------------------------------------
#define T100 99 // 100个时钟周期中断一次
#define T2Hz 20000 // 20000个时钟周期读取速度一次

//工作变量-------------------------------------------------------------------------
unsigned int uWork,uN,nCount,nCount1,nCount2,nCount3;
int nSSS,nJSSpeed,pwm;
int md,wc;

float a=0.6f,b=0.2f,c=0.1f,duk;
int ek,ek1,ek2,tz;
int nInput;
void PrintParameters();

//主函数---------------------------------------------------------------------------
main()
{
unsigned int nScanCode;
unsigned char cKey;
int speed[100],sp,lj;
float ljh;
char strInput[4];
int i,w1,w2,w3;

// 初始化工作变量
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=83; pwm=0;

InitDSPBoard(); // 初始化ICETEK-VC5416-A板
InitCTR(); // 初始化ICETEK-CTR板

// 设置显示参数和内容
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(); // 参数显示

SetForMotorB(); // 设置定时器等参数、启动中断

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();
PrintParameters();

}
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();
PrintParameters();
}
}
}
}
}
if ( nJSSpeed==0 ) // 读取速度标志
{
nJS

Speed=0;
nSSS=port8003; // 从端口读取速度计数
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
PrintParameters();
}
}
}
}
// 退出处理
TCR = 0x41f; // 关闭定时器
REGISTERCLKMD=0; // CPU降速到8兆
port8000=0; // 重新初始化ICETEK-CTR板
port8000=0x80;
port8000=0;
port8007=0x0c0; // 关闭直流电机B
exit(0);
}

// 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;
}

void interrupt time(void)
{
// PWM输出
SPSA1=1; // 设置McBSP1的SPCR2寄存器
SPSD1&=0xfffe; // 设置XRST位=0
SPSA1=0x0e; // 设置McBSP1的PCR1寄存器
uWork=SPSD1;
uWork|=0x2400; // 设XIOEN=1 FSXM=1,使能通用IO,FSR输出
if ( nCount1>uN ) uWork|=4; // 根据占空比设置FSR状态
else uWork&=0x0fffb;
SPSD1=uWork;

nCount++; nCount%=51200; // 键盘输入标志
nCount1++; nCount1%=100; // 占空比标志
// 速度采样脉冲1Hz方波
nCount2++; nCount2%=T2Hz; // 计数1秒
nJSSpeed++; nJSSpeed%=12000; // 读取速度标志
if ( nCount2==0 )
{
SPSA0=0x0e; // 设置McBSP0的PCR1寄存器
uWork=SPSD0;
uWork|=0x2800;
uWork^=0x8; // FSXP=~FSXP，BFSX0引脚状态取反
SPSD0=uWork;
}
}

void InitCTR()
{
port8000=0; //初始化CTR
port8000=0x80;
port8000=0;
port8007=0; // 关闭东西方向的交通灯
port8007=0x40; // 关闭南北方向的交通灯
uWork=port8002; // 清除键盘缓冲区
}

void InitDSPBoard()
{
REGISTERCLKMD=0; //DSP主频=8MHz
}

void SetForMotorB()
{
port8007=0x0c0;
SPSA1=1; // set McBSP1's SPCR2
SPSD1&=0xfffe; // set XRST=0
SPSA1=0x0e; // set McBSP1's PCR1
SPSD1|=0x2400; // set XIOEN=1 FSXM=1, Enable IO,FSR for output
SPSA0=0; // set McBSP0's SPCR1
SPSD0&=0xfffe; // set RRST=0
SPSA0=0x0e; // set McBSP0's PCR1
SPSD0|=0x1101; // set RIOEN=1 FSXM=1, Enable IO,CLKR for output,set Motor's direction to 1
SPSA0=1; // 设置McBSP0的SPCR2控制寄存器
SPSD0&=0xfffe; // 标志XRST=0
SPSA0=0x0e; // 设置McBSP0的PCR1寄存器
SPSD0|=

0x2800; // 设置：XIOEN=1 FSXM=1，使能通用I/O功能,FSX用于输出
SPSD0&=0x0fff7; // 设置：XIOEN=1 FSXM=1，使能通用I/O功能,FSX用于输出
port8007=0x0c4; // 使能直流电机B
do
{
SPSD0^=8;
uWork=port8003;
uWork=port8003;
uWork=port8003;
} while ( uWork&0x0ff );

asm(" ssbx INTM"); // 关中断，进行关键设置时不许打扰
uWork= PMST; PMST = uWork&0xff;
IMR = 0x8;
TCR = 0x41f; // 16个时钟周期计数一次
TIM = 0;
PRD = T100; // 周期=100次计数
TCR = 0x42f; // 开始计数
IFR = 0x8; // 使能中断
asm(" rsbx INTM"); // 开放中断
REGISTERCLKMD=0x1007; // DSP主频改为两倍PLL时钟=32MHz
}

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;
}

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;lfor ( 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();
}
int wwcc;
void PrintParameters()
{
wwcc=wc-md;
printf("测速[%3d] 设置[%3d] 误差[%+4d] PID调整量[%+3d] 占空

比[%3d%%]\n",
wc,md,wwcc,tz,pwm);
}