|
|
楼主 |
发表于 2009-12-25 18:12:20
|
显示全部楼层
源程序公开：
//--------------------------------------------------------------------------
//下推式磁悬浮源程序 2009.12.18
//liguang70217
//liguang70217@126.com
//http://liguang70217.blog.hexun.com/
//ICC-AVR application builder : 2009-12-17 17:27:22
// Target : M8
// Crystal: 12.000Mhz
//--------------------------------------------------------------------------
#include <iom8v.h>
#include <macros.h>
#include"BIT.h"
#define LED_A _PB0
#define LED_B _PB1
#define CA _PD0
#define CB _PD1
#define N 7
#define MAX_PID_OUTPUT 950
#define MAX_INTEGRATION_ERROR 100
#define X_DIRECTION_FLAG 0x01
#define Y_DIRECTION_FLAG 0x02
//--------------------------------------------------------------------------
typedef struct {
int targetValue;
int Kp;
int Ki;
int Kd;
int integrationError;
int prevError;
} PID;
PID xPID,yPID;
unsigned char direction;
unsigned int value_buf_x[N],value_buf_y[N];
unsigned char ix=0,iy=0;
unsigned int xpos,ypos; //AD转换后存放采集值
unsigned char HH;
unsigned int xError, yError;
unsigned int xPWM, yPWM;
//--------------------------------------------------------------------------
//------------延时子程序----------------------------------------------------
void delay(unsigned int h)
{
unsigned char j;
while(h--){
for(j=190;--j;) continue;
}
}
//--------------------------------------------------------------------------
// PID calculation routine
int calcPID(PID *pid, int error)
{
int output;
if (pid->Ki != 0)
{
pid->integrationError += error;
// Limit the maximum integration error
if (pid->integrationError > MAX_INTEGRATION_ERROR)
{
pid->integrationError = MAX_INTEGRATION_ERROR;
}
else if (pid->integrationError < -MAX_INTEGRATION_ERROR)
{
pid->integrationError = -MAX_INTEGRATION_ERROR;
}
}
output = pid->Kp * error + pid->Ki * pid->integrationError + pid->Kd * (error - pid->prevError);
// Limit the maximum output
if (output > MAX_PID_OUTPUT)
{
output = MAX_PID_OUTPUT;
}
else if (output < -MAX_PID_OUTPUT)
{
output = -MAX_PID_OUTPUT;
}
pid->prevError = error;
return output;
}
//--------------------------------------------------------------------------
void port_init(void)
{
PORTB = 0xff;
DDRB = 0xff;
PORTC = 0x00; //m103 output only
DDRC = 0x00;
PORTD = 0x00;
DDRD = 0xff;
}
//--------------------------------------------------------------------------
//TIMER1 initialize - prescale:1
// WGM: 7) PWM 10bit fast, TOP=0x03FF
// desired value: 2KHz
// actual value: 11.719KHz (82.9%)
void timer1_init(void)
{
TCCR1B = 0x00;//停止定时器
TIMSK |= 0x00;//中断允许
TCNT1H = 0x00;
TCNT1L = 0x00;//初始值
OCR1AH = 0x01;
OCR1AL = 0xFF;//匹配A值
OCR1BH = 0x01;
OCR1BL = 0xFF;//匹配B值
ICR1H = 0xFF;
ICR1L = 0xFF;//输入捕捉匹配值
TCCR1A = 0xA3;
TCCR1B = 0x09;//启动定时器
}
//--------------------------------------------------------------------------
//call this routine to initialize all peripherals
void init_devices(void)
{
//stop errant interrupts until set up
CLI(); //disable all interrupts
MCUCR = 0x00;
MCUCSR = 0x80;//禁止JTAG
GICR = 0x00;
port_init();
timer1_init();
// TIMSK = 0x04; //timer interrupt sources
SEI(); //re-enable interrupts
//all peripherals are now initialized
}
//-----------------------------------------------------------------------------
//ADC采样函数，采样第0通道信号，采样分辨率256
//x为ad端口号
unsigned int get_ad(unsigned char x)
{
union adres{ int y1; unsigned char adre[2]; }adresult;
unsigned int i;
ADMUX = 0xc0+x; /*基准AVCC、左对齐、通道7*/
ADCSRA = 0xC3; /*使能、开启、8分频*/
while(!(ADCSRA & (1 << ADIF))); /*等待*/
adresult.adre[0]=ADCL; //读取并存储A/D转换结果，A/D转换的结果通过共
adresult.adre[1]=ADCH; //用体的形式放入了变量y1中
ADCSRA &= ~(1 << ADIF); /*清标志*/
ADCSRA &= ~(1 << ADEN); /*关闭转换*/
return adresult.y1;
}
//--------------------------------------------------------------------------
//滑动平均滤波
unsigned int filter_x(void)
{
unsigned char count;
unsigned long sum=0;
value_buf_x[ix] = get_ad(0);
ix=ix+1;
if ( ix == N ) ix = 0;
for ( count=0;count<N;count++)
sum = sum + value_buf_x[count];
return (unsigned int)(sum/N);
}
//--------------------------------------------------------------------------
//滑动平均滤波
unsigned int filter_y(void)
{
unsigned char count;
unsigned long sum=0;
value_buf_y[iy] = get_ad(1);
iy=iy+1;
if ( iy == N ) iy = 0;
for ( count=0;count<N;count++)
sum = sum + value_buf_y[count];
return (unsigned int)(sum/N);
}
//-----------------------------------------------------------------------------
void main(void)
{
unsigned int ZD;
unsigned int ccc;
init_devices();
ZD=511;
// PID Parameter Initialization
xPID.Kp = 4;
xPID.Ki = 0;
xPID.Kd = 30;
xPID.integrationError = 0;
xPID.prevError = 0;
xPID.targetValue = ZD;
yPID.Kp = 4;
yPID.Ki = 0;
yPID.Kd = 30;
yPID.integrationError = 0;
yPID.prevError = 0;
yPID.targetValue = ZD;
while(1)
{
xpos=filter_x();
ypos=filter_y();
if(xpos>ZD)
{
xpos=xpos-ZD;
CA=1;
xError = xPID.targetValue - xpos;
xPWM = calcPID(&xPID, xError);
OCR1A = xPWM;
}
else
{
xpos=ZD-xpos;
CA=0;
xError = xpos;
xPWM = calcPID(&xPID, xError);
OCR1A = xPWM;
}
if(ypos>ZD)
{
ypos=ypos-ZD;
CB=1;
yError = yPID.targetValue - ypos;
yPWM = calcPID(&yPID, yError);
OCR1B = yPWM;
}
else
{
ypos=ZD-ypos;
CB=0;
yError = ypos;
yPWM = calcPID(&yPID, yError);
OCR1B = yPWM;
}
}
}
//---------------------------------------------------------------------------- |
|
[复制链接]
发表于 2009-12-24 18:46:30
