Miobot's Story


   AVR 기초 강좌 5-5 : 부록

(Tip4 Servo 제어)


Created Date: 2013.11.01
Modified Date: 2013.11.01
revision 1

키워드 : AVR, SERVO, WINAVR, AVR Studio

1. Servo Motor란?
DC Motor의 한 종류로서, PWM 제어 입력으로 특정 각 만큼 회전 후 위치를 유지하는 제어가 가능하다. 이는 모터 내부에 위치한 포텐셜미터(가변저항)를 포함한 컨트롤러가 PWM을 해석 후, 회전축에 연결된 포텐셜미터의 값이 그 와 같아질 때까지 모터를 회전시킴으로서 이루어진다.

일반적으로 0-180도(물론 360도 회전 가능한 것도 있다.) 회전하며, 이는 주기 20ms PWM 파형의 +Duty Pulse Width를 0.7-2.3ms 변화 시킴에 따라 비례, 제어된다.(1.5ms 에서 90도 회전)

 

(주의 : 주기 또는 최대, 최소 펄스폭의 크기가 위의 숫자에서 벗어날 수록 하모닉에 의한 소음, 진동 발생)

 

2. Servo Motor Control
우선, 개발환경은 Target : Atmega-128 16Mhz,  Compiler: WINAVR GCC,  IDE: AVR Studio 4.18 + SP3 임을 밝혀둔다.
 
20ms 주기의, 0.7-2.3ms 범위에서 펄스 폭을 가변 할 수 있는 코드를 작성해보자.
가장 손쉬운 방법은 TIMER0 오버플로우 인터럽트를 이용하는 방법이다. (TIMER에 관련된 설명은 앞서 표윤석님의 강좌 http://cafe.naver.com/openrt/14 를 참고하자.)
 
그럼 주기 20ms의 PWM을 0.1ms단위로 제어한다면 0.7-2.3까지 16단계로 밖에 제어가 되지 않는다. 좀더 세밀한 제어가 가능하려면 이보다 작은 단위로 제어가 이루어져야 한다. 20ms = 20,000us 이므로 10us 단위의 제어를 한다면 0.7-2.3ms → 700us-2300us. 160단계 제어가 가능해진다. 그렇다면 인터럽트의 주기는  10us 가 적당하겠다.
 
주파수 분주비 1을 사용할 경우, 1clk당 소요 시간(주기)은 1/16000000=0.0000000625초=0.0625us 이다.
0.0625×16=1us. 따라서 타이머가 16번 count에 1us가 소요되므로, 그 열 배인 160번 count에는 10us가 소요된다. 그렇다면 255-160+1=96(0x60)부터 160번 count하면, 159번 째 count값은 255, 160번 째 count에 오버플로우 인터럽트가 발생 할 것이다.(Timer0는 8bit, 0에서 2의 8제곱 - 1 까지 셀 수 있다.) 이 때 인터럽트서비스루틴 안에 1씩 증가하는 counter변수가 있다면, 그값이 70-230동안 High, 그 이 후 2000이 될 때까지 Low를 어디론가 출력하여, 서보의 입력으로 주면 될 것이다.
 
정리해보자면,
 
* 타이머 설정 : 주기 10us - 분주비 1, 카운트시작값 96(0x60)
 
* ISR(I nterrupt Service Routine )의 내용
1. 카운트시작값 96(0x60) 재설정.
2. 카운터 변수 +1
3. 카운터 변수의 값이 70-230이 될 때 까지 PORTA 0번 bit에 1 출력(임의의 사용가능한 아무 포트)
4. 그 이후 2000이 될 때까지 PORTA 0번 bit에 0 출력.
 
* main() : UART입력에 의해 펄스 폭을 가변하는 루틴 작성.
 
그럼 서보모터 제어 프로그램을 작성해보자.
3. 예제 코드
아래 코드의 1-7행의 주석은 위의 내용을 정리한 것으로 처음엔 define에 반영 사용해 왔으나 오실로스코프로 측정해 본 결과 오차가 발생함을 발견. 서보제어에는 문제가 없었지만, 측정값을 반영, 거의 오차없는 버젼을 아래 기록했음을 밝혀둔다.(오차범위 0.5% 이내) 
  1. // T_PWM : 20 ms
  2. // p_width : 0.7-2.3 ms
  3. // 20.0 ms = 10 us x 2000 ← PERIOD
  4. //  0.7 ms = 10 us x 70 ← MIN
  5. //  1.5 ms = 10 us x 150 ← 90˚
  6. //  2.3 ms = 10 us x 230 ← MAX
  7. // use timer0, uart1, PORTA
  8.  
  9. #include <stdio.h>             // 4 using printf()
  10. #include <avr/io.h>
  11. #include <avr/interrupt.h>
  12. #define PERIOD 1729           // T_pwm = 20 ms
  13. #define MAX 198               // MAX pulse width 2.3 ms
  14. #define MIN 60                // MIN pulse width 0.7 ms
  15. #define CENTER 129            // 90˚pulse width 1.5 ms
  16.  
  17. volatile unsigned int p_width = 0;
  18. volatile unsigned int duty = CENTER;
  19.  
  20. ISR(TIMER0_OVF_vect)
  21. {
  22.     TCNT0 = 0x60;            // (time/ 1 count)=0.0625 us, 0.0625 x 16 = 1us
  23.                              // 10us = 0.0625 x 16 x 10 = 0.0625 x 160
  24.      p_width++;         
  25.      if(p_width <= duty) PORTA |= 0x01; // output high until p_width = duty
  26.      else PORTA &= ~(0x01);             // output low until p_width = PERIOD
  27.  
  28.       if(p_width >= PERIOD) p_width = 0;
  29. }
  30.  
  31. void init_timer0(void)
  32. {
  33.      TCCR0 = 0x00; // stop
  34.      ASSR = 0x00;  // set async mode
  35.      TCNT0 = 0x60; // set count start from 96 to 255 + 1
  36.      TCCR0 = 0x01; // Precaler=1/1, 16,000,000/1=16000,000Hz T=1sec/16000,000=0.0625us
  37.                    // 0.0625us×16=1us, 10us=0.0625us×160
  38.      TCNT0 = 0xfe// after count twice, TIMER0 OVF
  39. }                                      
  40.  
  41. static int putch_uart1(char message, FILE *stream) // 4 using printf()
  42. {
  43.     if (message == '\n')
  44.     putch_uart1('\r', stream);
  45.     while((UCSR1A & 0x20) == 0x00);
  46.     UDR1 = message;
  47.     return 0;
  48. }
  49.  
  50. int getch_u1(void)
  51. {
  52.     while ((UCSR1A & 0x80) == 0);
  53.     return UDR1;
  54. }
  55.  
  56. void init_port(void)
  57. {
  58.     PORTA = 0x00; DDRA = 0x00; // Port A Initialize
  59.     PORTB = 0x00; DDRB = 0x00; // Port B Initialize
  60.     PORTC = 0x00; DDRC = 0x00; // Port C Initialize
  61.     PORTD = 0x00; DDRD = 0x00; // Port D Initialize
  62.     PORTE = 0x00; DDRE = 0x00; // Port E Initialize
  63.     PORTF = 0x00; DDRF = 0x00; // Port F Initialize
  64.     PORTG = 0x00; DDRG = 0x03; // Port G Initialize
  65. }
  66. void init_uart1(void)          // UART1 initialize. baud rate: 9600
  67. {                              // data: 8 bit, parity: Disabled
  68.     UCSR1B = 0x00;             // disable while setting baud rate
  69.     UCSR1A = 0x00;
  70.     UCSR1C = 0x06;
  71.     UBRR1L = 0x67;             //set baud rate lo
  72.     UBRR1H = 0x00;             //set baud rate hi
  73.     UCSR1B = 0x18;
  74. }
  75. void init_devices(void)
  76. {
  77.     cli();
  78.     init_port();
  79.     init_uart1();
  80.     init_timer0();
  81.     fdevopen(putch_uart1,0);        // 4 using printf();
  82.     TIMSK = 0x01;
  83.     sei();
  84. }
  85.  
  86. int main(void)
  87. {
  88.     unsigned int duty_ctrl = CENTER; // 90 degree
  89.     char ch;
  90.     init_devices();
  91.     DDRA = 0xff; // PortA Output
  92.     printf("\nServo Motor test....\n");
  93.     printf("type '<' or '>' to control servo\n");
  94.     while(1)
  95.     {
  96.         ch = getch_u1();
  97.         if((ch == '.') || (ch == '>'))   duty_ctrl++;
  98.         if((ch == ',') || (ch == '<'))   duty_ctrl--;
  99.         if(duty_ctrl >= MAX) duty_ctrl = MAX;
  100.         if(duty_ctrl <= MIN) duty_ctrl = MIN;
  101.         duty = duty_ctrl;
  102.         printf("duty : %3d\n", duty_ctrl);
  103.     }
  104. }

  지난  7월 한 달 윤석님의 이 강좌로 AVR에 대해 많이 배웠습니다.  핵심만 추려 설명한 강좌이다 보니, 그 당시 이런 예제는 타카페를 뒤져 참고했던 기억도 나고, 지금 서보모터를 만지느라 소스 찾아서 보다가 생각났을 때, 몇 자 적어 봅니다. 틈틈이 몇 가지 더 정리되는 대로 올려 보도록 하겠습니다.

 

End.

written by Ground Zero


이 저작물은 크리에이티브 커먼즈 저작자표시-비영리 3.0 Unported 라이선스에 따라 이용할 수 있습니다.


Posted by Miobot