프로그래밍이라고는 10여년 전에 정보처리기능사 딴다고 비주얼베이직 깨작거린거랑 5년쯤 전에 교양? 어쨌든 C++ 날림으로 한학기 기초만 배운것 뿐인데 졸업논문쓰러 들어간 제어공학 연구실에선 대뜸 적응형퍼지 제어기를 짜야된다 그러고.... 여차저차해서 PID로 타협이되어 코딩을 하게 됐습니다.

외부인터럽트다 뭐다, 공개된 소스도 별로 없고 그나마 있는것들은 정보도 여기저기 가려서 띄엄띄엄공개해서(물론 제가 까막눈이라 모르는것이겠지만) 쓸수도 없고, 해서 독학해서 PID 제어 코드를 짜는데까지는 성공했습니다.

해서 일단 모터 2개를 놓고 하나는 오픈루프, 하나는 PID제어로 돌리고 차이를 보려고 했는데.... 별 차이가 안보이네요-_-;

AVR 카페같은데 질문을 올려도 \'실물이 아니라 시뮬레이션이라 안되는 것일 수도 있다\'는 댓글정도 외에는 조언해 주는 분도 안계시고.... 코드가 안좋다거나 하는 지적은 많이 받지만--;

혹시나 조언을 얻을 수 있을까 해서 프로그래밍 갤러리에 질문 올립니다.

첨부한 그림은 프로테우스 회로도이고, 주석에도 써놨지만 엔코더 모터는 Current입력을 해보니 최고 920rpm정도로 올라가고, 엔코더는 24PPR입니다.

KP KI KD는 처음엔 1 0 0으로 시작했다가 별 차이가 안보여서 어처구니없는 숫자를 넣어본겁니다.

혹시 코드에 무슨 문제가 있는지 아시는분은 조언해주시면 감사하겠습니다.

#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>
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<d;i++) _delay_ms(1);
}