마이크로프로세서 HOT팬틸트 제작( 2개 서보모터 따로 제어) 질문입니다.

서보모터 2개를 이용해서 팬틸트를 만드려고 합니다.
일단은 시리얼 통신으로 팬틸트를 제어해보고자 해서 예제를 참고해서 짠 소스입니다.

원래 타이머 하나로 서보모터 2개를 동시에 제어하는거 까지는 성공했으나..
2개를 각각 따로 제어하는게 잘 안되서 여기저기 찾아도 보고 생각해봐서 짠 소스입니다만,

TIMSK  = 0x03;
으로 했을 시엔 TIMER0만 동작하고
TIMSK = 0x04;
로 설정했을시엔 TIMER1만 동작합니다.

아직 소스 이해가 부족해서 TIMSK 부분 값을 어떻게 설정해야 될지 몰라서 질문글 올립니다..


//ICC-AVR application builder : 2007-04-20 오후 2:39:41
// Target : M128
// Crystal: 16.000Mhz

#include <iom128v.h>
#include <macros.h>
#include <stdio.h>

#define SERVOMIN 0
#define SERVOMAX 200
//#define SERVOMAX 100
#define SERVOPORT PORTF
#define SERVODDR DDRF

#define MOVMAX 23
#define MOVMIN 20

#define VALUEMIN 6
#define VALUEMAX 24

#define ARRAYMAX 16

volatile unsigned int servoTime1 = 0;
volatile unsigned int servoTime2 = 0;
volatile int defValue1;
volatile int defValue2;
int i;

// printf 함수 사용시 추가할 것.
int putchar(char c)
{
    while (((UCSR0A>>UDRE0)&0x01) == 0) ;  // UDRE, data register empty
       
    UDR0 = c;
    return c;
}

// scanf 함수 사용시 추가할 것.
int getchar(void)
{
    while ((UCSR0A & 0x80) == 0);
    return UDR0;
}

void port_init(void)
{
 PORTA = 0x00;
 DDRA  = 0x00;
 PORTB = 0x00;
 DDRB  = 0x00;
 PORTC = 0x00; //m103 output only
 DDRC  = 0x00;
 PORTD = 0x00;
 DDRD  = 0x00;
 PORTE = 0x00;
 DDRE  = 0x00;
 PORTF = 0x00;
 DDRF  = 0xFF;
 PORTG = 0x00;
 DDRG  = 0x00;
}

//TIMER0 initialize - prescale:64
// WGM: Normal
// desired value: 20uSec
// actual value: 20.000uSec (0.0%)
void timer0_init(void)
{
 TCCR0 = 0x00; //stop
 ASSR  = 0x00; //set async mode
 TCNT0 = 0xFB; //set count
 OCR0  = 0x05;
 TCCR0 = 0x04; //start timer
}

#pragma interrupt_handler timer0_ovf_isr:iv_TIM0_OVF
void timer0_ovf_isr(void)
{
 unsigned char n = 0;
 
 TCNT0 = 0xFB; //reload counter low value

 if(servoTime2 >= SERVOMAX){
 servoTime2 = SERVOMIN;
 }
 servoTime2++;

 if(defValue2 > servoTime2)
 SERVOPORT = 0x01;
 else
  SERVOPORT = 0x00;
}

//TIMER1 initialize - prescale:64
// WGM: 0) Normal, TOP=0xFFFF
// desired value: 20uSec
// actual value: 20.000uSec (0.0%)
void timer1_init(void)
{
 TCCR1B = 0x00; //stop
 TCNT1H = 0xFF; //setup
 TCNT1L = 0xFB;
 OCR1AH = 0x00;
 OCR1AL = 0x05;
 OCR1BH = 0x00;
 OCR1BL = 0x05;
 OCR1CH = 0x00;
 OCR1CL = 0x05;
 ICR1H  = 0x00;
 ICR1L  = 0x05;
 TCCR1A = 0x00;
 TCCR1B = 0x03; //start Timer
}

#pragma interrupt_handler timer1_ovf_isr:iv_TIM1_OVF
void timer1_ovf_isr(void)
{
 unsigned char n = 0;
 
 TCNT1H = 0xFF; //reload counter high value
 TCNT1L = 0xFB; //reload counter low value

 if(servoTime1 >= SERVOMAX){
 servoTime1 = SERVOMIN;
 }
 servoTime1++;
 
 if(defValue1 > servoTime1)
 SERVOPORT = 0x03;
 else
  SERVOPORT = 0x01;
}

//UART0 initialize
// desired baud rate: 115200
// actual: baud rate:111111 (3.7%)
// char size: 8 bit
// parity: Disabled
void uart0_init(void)
{
 UCSR0B = 0x00; //disable while setting baud rate
 UCSR0A = 0x00;
 UCSR0C = 0x06;
 UBRR0L = 0x08; //set baud rate lo
 UBRR0H = 0x00; //set baud rate hi
 UCSR0B = 0x18;
}

//call this routine to initialize all peripherals
void init_devices(void)
{
 //stop errant interrupts until set up
 CLI(); //disable all interrupts
 XDIV  = 0x00; //xtal divider
 XMCRA = 0x00; //external memory
 port_init();
 uart0_init();
 timer0_init();
 timer1_init();
 SERVODDR = 0xFF;

 MCUCR = 0x00;
 EICRA = 0x00; //extended ext ints
 EICRB = 0x00; //extended ext ints
 EIMSK = 0x00;
 TIMSK = 0x04; //timer interrupt sources
 ETIMSK = 0x00; //extended timer interrupt sources
 SEI(); //re-enable interrupts
 //all peripherals are now initialized
}

void delay(int cnt){
 int i, j;
 
 for(i=0; i < cnt; i++){
  for(j=0; j < 1000; j++){
   ;
  }
 }
}

void main(){
 int i=24;
 unsigned char ch;
 init_devices();
 printf("Testing Servo\r\n");
 
 defValue1 = 74;
 defValue2 = 74;
 while(1){
   scanf("%c", &ch);
   printf("Value:%d",defValue1);
    printf("Value:%d",defValue2);
   if((ch == 'a') || (ch == 'A')){
   defValue1++;
   }
  
   if((ch == 'q') || (ch == 'Q')){
   defValue1--;
   }
  
   if((ch == 's') || (ch == 'S')){
   defValue2++;
   }
  
   if((ch == 'w') || (ch == 'W')){
   defValue2--;
   }    
  
  if(defValue1 <=32) defValue1 = 32;
   if(defValue1 >=114) defValue1 = 114;
  if(defValue2 <=32) defValue2 = 32;
   if(defValue2 >=114) defValue2 = 114;
  }
} 


서보 모터 두개를 각각 따로 제어해야 하는데 똑같이 움직일 수는 있어도
따로 움직이는게 어렵네요 타이머 하나로 쓰려니까 계속 떨리는 현상이 생겨서 이렇게 짜봤는데..
타이머 두개가 동시에 구동이 안되서 어떻게 해야 될지 모르겠습니다.
포트 2개를 쓰지 않아도 타이머를 여러개 쓰면 각각 따로 제어가 가능할 것 같은데..
시작한지 얼마 안되서 잘 모르겠네요..


요점은
서보모터 2개를 각각 따로 제어하기 위해서
위 소스에서 고쳐야 할 부분.. 알려주시면 감사하겠습니다..