AVR 单片机PID 控制电机程序
2011-04-10 20:53
/*****************************************************
Project : PID_motor
Version : 第一版
Date : 2011-3-30
Author : 卢云
Company : 三峡大学电子设计创新实验室
Comments:
Chip type : ATmega16L
Program type : Application
Clock frequency : 4.000000 MHz
Memory model : Small
External SRAM size : 0
Data Stack size : 256
*****************************************************/
#include
#include
#include
#include
sfrw ICR1=0x26; //
义)
unsigned int icp_v1;
unsigned int icp_v2; 补充定义16位寄存器ICR1地址为0x26(mega16.h中未定
unsigned char icp_n;
unsigned char max_icp; //量程定义字
bit icp_ok;
bit time_4ms_ok;
bit freq_ok;
bit begin_m; //
bit full_ok; //
long fv;
struct PID
{
unsigned int SetPoint; //
unsigned int Proportion; //
unsigned int Integral; //
unsigned int Derivative; //
unsigned int LastError; // Error[-1]
unsigned int PrevError; // Error[-2]
unsigned int SumError; // Sums of Errors
};
struct PID spid; // PID Control Structure
long rout; // PID Response (Output)
long rin; // PID Feedback (Input)
void PID_Init(struct PID *pp)
{
memset ( pp,0,sizeof(struct PID)); //定时器1溢出，重新测量标志字 定时器1溢出 设定目标 Desired Value 比例常数 ProportionalConst 积分常数 Integral Const 微分常数 Derivative Const 全部初始化为0
}
unsigned int PID_Calc( struct PID *pp, unsigned int NextPoint )
{
unsigned int dError,Error;
Error = pp->SetPoint - NextPoint; // 偏差
pp->SumError += Error; // 积分
dError = pp->LastError - pp->PrevError; // 当前微分
pp->PrevError = pp->LastError;
pp->LastError = Error;
return (pp->Proportion * Error // 比例项
+ pp->Integral * pp->SumError // 积分项
+ pp->Derivative * dError); // 微分项
}
// Timer 2 比较匹配中断服务，4ms 定时
interrupt [TIM2_COMP] void timer2_comp_isr(void)
{
#asm("sei") // 开放全局中断, 允许中断嵌套
time_4ms_ok = 1;
}
// Timer 1 溢出中断服务
interrupt [TIM1_OVF] void timer1_ovf_isr(void)
{
full_ok = 1;
}
// Timer 1 输入捕捉中断服务
interrupt [TIM1_CAPT] void timer1_capt_isr(void)
{
if (icp_n >= max_icp) // 第N 个上升沿到
{
icp_v2 = ICR1; // 记录第N 个上升沿时间
TIMSK = 0x80; // 禁止T/CI输入捕捉和溢出中断
icp_ok = 1;
}
else if (icp_n == 0)
{
icp_v1 = ICR1; // 记录第1个上升沿时间
}
icp_n++;
}
void main(void)
{
unsigned int icp_1,icp_2;
PORTB=0x00;
DDRB=0x08;
PORTD=0x40; // PD6(icp)输入方式，上拉有效
// T/C0 定时初始化
TCCR0=0x79; // Mode: Fast PWM top=FFh
TCNT0=0x00; // OC0 output: Non-Inverted PWM
OCR0=0xff;
// T/C1 计数初始化
TCCR1B = 0x41; // T/C1正常计数方式，上升沿触发输入捕捉，4M/1计数时钟
TIMSK = 0xA4; // 允许T/C2比较匹配中断，允许T/C1输入捕捉、溢出中断
// T/C2 定时初始化
TCCR2=0x0C; // 内部时钟，64分频（4M/64=62.5KHz），CTC 模式
OCR2=0xff; // OCR2 = 0xf9(249),(249+1)/62.5=4ms
icp_n = 0;
max_icp =1; //定义上升沿的位序
#asm("sei") // 开放全局中断
INIT_FYD();
Show_Text(3,0,chinese); //(列，行，显示字)
PID_Init(&spid); // Initialize Structure
spid.SetPoint = 168; //设定目标 Desired Value
spid.Proportion = 6; //比例常数 Proportional Const
spid.Integral =5; //积分常数 Integral Const
spid.Derivative =2; //微分常数 Derivative Const
FM_Num(0,1,spid.Proportion);
FM_Num(3,1,spid.Integral);
FM_Num(6,1,spid.Derivative);
FM_Num(0,2,spid.SetPoint);
while (1)
{
if (icp_ok == 1) // 完成一次测量
{
if (icp_v2 >= icp_v1) // 计算N 个上升沿的时钟脉冲个数， icp_2 = icp_v2 - icp_v1; //两次连续的ICR1的差值
else
icp_2 = 65536 - icp_v1 + icp_v2; //定时器1溢出，加上65535 if (!(icp_v2 >= icp_v1 && full_ok)) // 有溢出，数据无效
{
//if (icp_2 == icp_1) // 两次个数相等，测量有效 {
fv = 1000000 * (long)max_icp / icp_2; // 换算成频率值
freq_ok = 1; //频率换算完成
if (fv > 4000)
{
max_icp = 20; // 如果频率大于4Khz ，N=64 }
else
{
max_icp = 1; // N=1
}
}
}
else
max_icp = 1; // 有溢出，N=1 icp_1 = icp_2;
icp_ok = 0;
begin_m = 1;
}
if (time_4ms_ok) //定时器2，定时到 {
if(freq_ok) //判断频率是否换算完成 {
rin=fv;
FM_Num(0,3,rin);
rout = PID_Calc ( &spid,rin );
rout=rout/100;
FM_Num(3,3,rout);
if(rout>100&&rout
OCR0=rout;
freq_ok = 0;
}
else if(begin_m)
{
icp_n = 0; // 开始新的一次测量，
full_ok = 0; // 清除溢出标志
TIFR = 0x24; // 清除可能存在的输入捕捉、溢出中断标志位
TIMSK = 0xa4; // 开启T/C1输入捕捉、溢出中断允许
begin_m = 0;
}
time_4ms_ok = 0;
}
} }