新搞的servo控制算法
基于pic16,比较简单的滤波算法和pid算法。
因为只用了积分所以大大降低了难度，输出为频率脉冲。
其实是个数字化的vf变换器。
过几天到机床上去试试效果，在确定系数k和相关的参数。
程序如下，以供参考。
#include<pic.h>
__config(0x3b32);
#define uchar unsigned char
#define uint unsigned int
uint ad_val();
void init();
void int_init();
void delay(uint x);
const uchar table[]={0xd7,0x12,0xcb,0x5b,0x1e,0x5d,0xdd,0x13,0xdf,0x5f};
void dsp(uchar num1,uchar num2,uchar num3);
void sc();
void vf();
uint lednum;
int adval,val;
uint adtemp,advaltemp;
uchar adcunt,adflg;
uchar bai,shi,ge,dlv;
uint time_cunt;//vf变换时间
uchar flg_ad_en;//ad标记
uchar flg_pid_en;//pid标记
uchar flg_vf_en;//vf变换标记
uchar flg_dl_en;//数字滤波标志
uchar flg_ch;
uchar pid_cunt;
float fval;//ad值
int pid_val_g;//给定
int pid_val_b;//反馈
int d_fadval;
int fa,fb,fc,fd;
uint a1,a2,a3,a4 ;
void main()
{
init();//io初始化
int_init();//中断初始化
adtemp=0;//清寄存器
dlv=20;//数字滤波器赋值
time_cunt=10 ;
while(1)
{
if(flg_ad_en==1)
{
rc1=0;
flg_ad_en=0;//表示ad采样结束
val=adresh;
adval=val<<8|adresl;
fval=adval/1023*50;
//******如果flg_ad_en标记为1，那么求ad转换的值***//
if(5<=fval<20)//此处20为分压所得的数据
{
d_fadval=d_fadval+fval;//浮点数累加
dlv--;
if(dlv==0)
{
dlv=20;
d_fadval= d_fadval/20;//求平均数
pid_val_b=d_fadval*2;//补偿分压值
d_fadval=0;//累加数归零
flg_pid_en=1;//pid允许位
}
}
}
//******以上为数字滤波程序**************//
if(flg_pid_en==1)
{
pid_cunt=1;
flg_pid_en=0;//允许位归零
pid_val_g=16;
fa=pid_val_g-pid_val_b;
fb=fa*4;
}
}
}
void interrupt time2()//中断处理
{
if(tmr2if==1)
{
tmr2if=0;
time_cunt--;
if(time_cunt==0)
{
time_cunt=100-fb;//16*16*0.4us*5,这里需要设置变量
if(rc0==1)
{
rc0=0;
}
else
{
rc0=1;
}
}
}
if(adif==1)
{
adif=0;
flg_ad_en=1;
adgo=1;
rc1=1;
}
}
void init()
{
trisa=0x01;
trisb=0;
trise=0;
trisd=0;
trisc=0;
porte=0;
portd=0;
portb=0;
porta=0;
porte=0x06;
delay(2);
porte=0;
cmcon=0x07;//关比较器
adcon0=0x81;//1/64，ch0，power on
adcon1=0xce;//数据右对齐an0为模拟输入。
adgo=1;
//-----ad初始化----------
}
void int_init()
{
gie=1;//开总中断
adie-1;//开ad中断
peie=1;//开外围中段
tmr2ie=1;//开定时器2中断
t2con=0x2a;//前后分频5/1（5*4），定时器2电源关闭。
tmr2if=0;//清中断标志位
tmr2on=1;//开定时器2
pr2=0x01;//十进制数20（5*4*50=1000us）
}
void dsp(uchar num1,uchar num2,uchar num3)
{
portb=table[num1];//显示第一个数码管
portd=0x04;
sc();
delay(2);
portb=table[num2];//显示第二个数码管
portd=0x02;
sc();
delay(2);
portb=table[num3];//显示第三个数码管
portd=0x01;
sc();
delay(2);
}
void sc()//数据所存
{
porte=0;
delay(2);
porte=0x06;
delay(2);
porte=0;
delay(2);
}
void delay(uint x)//延时程序
{
uint a,b;
for(a=x;a>0;a--)
for(b=110;b>0;b--);
}
