AVR을 이용한 PID 제어 소스

 

일단 여기 올려둔다. 복사해서 넣으면 안되는 소스임을 미리 밝힌다.

단 아래내용으로 AVR을 이용해 PID를 어떻게 만들어야 하는지 참고자료로 사용할 순 있겠다.

#include
#include
#include
typedef unsigned char byte;
typedef unsigned int word;
#define cbi(REG8,BITNUM) REG8 &= ~(_BV(BITNUM))
#define sbi(REG8,BITNUM) REG8 |= _BV(BITNUM)
void delay (int d);
volatile unsigned int count0 = 0;
volatile unsigned int count1 = 0;
volatile unsigned long encoder_T0 = 0;
volatile unsigned long encT_0 = 0;
volatile float motor_signal0;

void ex_int(void) { // external interrupt initialization
SREG |= 0x80; // enable external interrupt
EIMSK |= 0x03; // 0,1 enable
EICRA = 0x0a; // falling edge
}

ISR(INT0_vect) { // INT0 핀에 엔코더 펄스가 입력되면 count0 증가, 주기는 500us
count0++;
_delay_us(500);
}

ISR(INT1_vect) { // INT1 핀에 엔코더 펄스가 입력되면 count1 증가, 주기는 500us
count1++;
_delay_us(500);
}

void ex_int0(void){ // TCNT1과 count0을 이용해 모터의 회전 주기 검출
encoder_T0= count0*1000 + TCNT1;
TCNT1=0;
count0=0;
encT_0 = encoder_T0*48;//encoder_T의 2배는 A펄스의 주기이고, 엔코더는 1회전당 24펄스. 따라서 encT_0은 모터0이 1회전하는데 걸리는 시간.
}

void pid0(void) { // PID 제어 함수

volatile float error_funct;
volatile float old_error_funct;
volatile float old_error_funct2;
volatile float desired;
volatile float measured;
volatile float old_motor_signal;
volatile double KP = 90.3;
volatile double KD = 90.1;
volatile double KI = 200.0;
volatile float delta_t = 0.005;
volatile int limit = 100;
volatile int pwmduty;

pwmduty = OCR3A;
desired = (pwmduty/256)*920; // 목표 회전수 계산. MAX rpm = 920.
measured = (1/encT_0)*60; // ex_int0에서 검출한 회전주기를 이용해 rpm 검출

// calculate the motor signal according the PID equation.
// the derivative and the integral are approximated using simple linear approximations.

error_funct = desired - measured;
motor_signal0 = old_motor_signal + KP * (error_funct - old_error_funct) + KI * delta_t * (error_funct + old_error_funct) / 2 + (KD / delta_t) * (error_funct - 2 * old_error_funct + old_error_funct2);
// 선형근사를 이용한 출력 계산식.
if (motor_signal0 < 0) { // limiting the output
motor_signal0 = 0;
}
if (motor_signal0 > limit) {
motor_signal0 = limit;
}

old_motor_signal = motor_signal0; // update
old_error_funct2 = old_error_funct;
old_error_funct = error_funct;

}

int main(void) {
unsigned char button;

ex_int();
DDRG = 0xff; PORTG = 0x01;//Motor CW
DDRE = 0xff; PORTE = 0x00;//Output
DDRF = 0xff; PORTF = 0x00;//Switch
DDRD = 0x00; PORTD = 0x00;//External interrupt

OCR3A = 128; //duty ratio 50%
OCR3B = 128;
TCCR3A = 0xa9;
TCCR3B = 0x05;

sei();

do{
ex_int0();
pid0();
button = PINF;
switch (button) {
case 0x01:
if (OCR3A<246) OCR3A = OCR3A + 10 + motor_signal0;
if (OCR3B<246) OCR3B+=10;
break;
case 0x02:
if (OCR3A>9) OCR3A = OCR3A - 10 - motor_signal0;
if (OCR3B>9) OCR3B-=10;
break;
case 0x04:
if (OCR3A<250) OCR3A = OCR3A + 5 + motor_signal0;
if (OCR3B<250) OCR3B+=5;
break;
case 0x08:
if (OCR3A>5) OCR3A = OCR3A - 5 - motor_signal0;
if (OCR3B>5) OCR3B-=5;
break;
}
delay(20);
}while(1);

}

void delay (int d) {
for (int i=0;i
}


http://magicom9.tistory.com/3