刚做好的servo算法
用来跟踪主轴速度
本来采用dsp c2000 lf2407 但是要用杜邦线 飞线不太可靠
后来就用自己做的pic开发板 速度20m
主轴刷新速度每秒15次
一个完整的机床系统包含数控系统 mosc spi总线（为了减少线的数量采用spi总线）
总线校验 命令 数据 解码系统等等
其中单轴数控系统 mosc spi相关 spi decode 我用了一片fpga cyclone2 ep2c5来完成的使用verilog完成
以下代码只含servo的部分c代码 以供坛内相互学习借鉴
#include<pic.h>
__config(0x3b32);
//**************************************************
// key定义
//**************************************************
#define uchar unsigned char //无符号字符数据声明
#define uint unsigned int //无符号整形数据声明
#define ki 19 //积分常数，用于计算pi
#define scl rb6
#define sda rb7
#define rst rb5
#define en rb4
#define s1 rc4
#define s2 rc5
//**************************************************
// command定义
//**************************************************
#define stf 0x02 //crc校验位
#define ton 0x81
#define toff 0x82
#define ds 0x83
#define dp 0x84
#define load 0x85
//**************************************************
// 变量申明
//**************************************************
volatile long int spi_data; //合成的24bit数据
volatile long int command; //spi命令数据
volatile long int st; //校验数据
volatile long int data; //spi传输的数据段
volatile uchar point; //bit传输指针
volatile uchar cnt; //按键状态灯
volatile long int crc; //crc校验位
volatile long int shift; //数据移位暂存
volatile long int data_temp; //数据移位暂存
static long int bank1 aa;
static long int bank1 bb;
volatile static long int bank1 cc;
volatile static long int bank1 dd;
//**************************************************
// ad变换变量定义
//**************************************************
volatile uint advaltemp=0; //ad数据中转寄存器
volatile uint val_temp=0; //ad数据中间寄存器
volatile uint val_dr=0; //ad数据中间寄存器
volatile float val_dt=0; //ad数据中间寄存器
//***************************************************
// 主轴速度变量定义
//***************************************************
// 速度寄存器
volatile long int speed;
//***************************************************
// 计数及标记变量定义
//***************************************************
uchar i_cunt=0; //累加计数器
uchar p_cunt=0; //积分时间计数器
uchar s_cunt=0; //电机速度寄存器---中断计数器
uchar flag_d=0; //方向信号
uchar flag_s=0; //停止标记
//****************************************************
// pi运算变量定义
//****************************************************
volatile long int val_t=0; //数字滤波后的数据
volatile long int val_g=700; //伺服给定
volatile long int val_p=0; //积分后的偏差数据
volatile long int val_tp=0; //单次偏差数据
//****************************************************
// 显示函数用变量定义
//****************************************************
const uchar table[]={0xd7,0x12,0xcb,0x5b,0x1e,0x5d,0xdd,0x13,0xdf,0x5f};//显示表--------------
//*****************************************************
// 函数声明
//*****************************************************
void init(void); //io和系统初始化函数
void delay(uint x); //延时函数
void int_init(void); //中断初始化函数
uint ad_val(void); //ad转换函数
void dsp(uchar num1,uchar num2,uchar num3);//显示函数（led)-------------------
void sc(void); //数据所存---------------------
int pi( void); //pi求解函数
void spi_rst(void);
void write(long int data_w); //写函数（spi发送函数）
void spi_rst(void); //spi总线复位函数
void data_coll(uchar com,uint data_c); //数据合成函数（用来合成发送的数据）
//******************************************************
void main(void)
{
init(); //io初始化,ad初始化
int_init(); //中断初始化
spi_rst();
val_t=0;
val_tp=0;
val_p=0;
val_g=300; //伺服给定70v
speed=51000; //初始速度设定 步进距离7mm/min
nop();
spi_rst();
data_coll(ds,speed); //数据合成
data_temp=spi_data; //数据转移
write(data_temp); //数据发送
delay(2);
while(1)
{
pi();
nop();
}
}//end main
uint ad_val()//ad采样程序
{
uint val;
adgo=1;//启动ad
while(adgo);//等待ad转换结束
val_dr=adresh;//00000000_00000011
advaltemp=(val_dr<<8)|adresl;//10bit ad数据
val_dt=advaltemp/1023.0*3.3;//补偿返回值丢失的精度
val=val_dt*200;
return (val);
}
void init()
{
//****************************************
// ad及其io端口初始化
//****************************************
trisa=0x01;
trisb=0x00;
trise=0;
trisd=0x00;
trisc=0xff;
porte=0;
portd=0;
portb=0;
porta=0;
porte=0x00;
delay(2);
porte=0;
sc();//所存数据 端口清零
pspmode=0;//并行口关闭
cmcon=0x07;//关比较器
adcon0=0x81;//1/64，ch0，power on
adcon1=0xce;//数据右对齐an0为模拟输入。
//********************************************
// 定时器1和2初始化
//********************************************
//----定时器2初始化---------------------------
t2con=0x21;//预分频器4，后分频器5.total 20 tmr2关闭
pr2=0xf9;//pr2+1=250 pr2=249;
tmr2on=1;
//----定时器1初始化--------------------------
t1con=0x34;//定时器控制初始化
tmr1h=0xc3;//定时器赋值
tmr1l=0;//定时器赋值
tmr1on=1;//定时器打开
}
void int_init()//
{
gie=0; //closed总中断
peie=1; //开外围中段
tmr2ie=1;//允许t2中断
tmr1ie=1;//允许t1中断
//-----中断寄存器设置--------
tmr2if=0;//t2中断标志清零
tmr1if=0;//t1中断标志清零
//------中断标记清零----------
}
int pi()//pi求解函数
{
for(p_cunt=0;p_cunt<=ki;p_cunt++) //此处决定积分时间 积分时间由宏定义完成
{
val_t =ad_val();
val_tp=val_g-val_t;//计算偏差,单次偏差
val_p=val_p+val_tp;//求积分，ki次偏差数据和
}
if(val_p>=0)//给定大于反馈，电压低
{
speed=speed+val_p;//主轴加速
val_t=0;
val_tp=0;
val_p=0;
if(speed<=1000)//限制低速速度
{
speed=500;
rd7=1;
rd6=0;
sc();
nop();
}
if(speed>=64285)//限制高速速度
{
speed=64000;
rd6=1;
rd7=0;
sc();
nop();
}
}
if(val_p<0)//给定小于反馈，电压高
{
val_p=val_p*-1;//数据变换,求绝对值
speed=speed-val_p;//主轴减速
val_t=0;
val_tp=0;
val_p=0;
if(speed<=1000)//限制低速速度
{
speed=500;
rd7=1;
sc();
nop();
}
if(speed>=64285)//限制高速速度
{
speed=64000;
rd6=1;
sc();
nop();
}
}//end if(val_p<0)
spi_rst();
data_coll(ds,speed);
data_temp=spi_data;
write(data_temp);
delay(2);
}//end pi
void write(long int data_w)
{
shift=data_w;
data_temp=data_w&0x800000;
data_temp=data_temp>>23;
nop();
for(point=24;point>0;point--)//发送24bit数据
{
if(data_temp==1)
{
sda=1;
delay(2);
scl=1;
delay(2);
}
else
{
sda=0;
delay(2);
scl=1;
delay(2);
}
delay(2);
shift=shift<<1;
data_temp=shift;
data_temp=data_temp&0x800000;
data_temp=data_temp>>23;
delay(2);
scl=0;
sda=0;
}
}
void data_coll(uchar com,uint data_c)
{
crc=0x02;//校验位
crc=crc<<22;
command=com;//写on
command=command&0x3f;
command=command<<16;
data=data_c;
spi_data=data|crc|command;
}
void delay(uint x)//延时程序
{
uint a,b;
for(a=x;a>0;a--)
for(b=110;b>0;b--);
}
void spi_rst(void)
{
rst=0;
delay(1);
scl=1;
delay(1);
scl=0;
rst=1;
}
void sc()//数据所存
{
porte=0;
delay(2);
porte=0x06;
delay(2);
porte=0;
delay(2);
}
