각도제어를 20ms 속도제어를 4ms마다 측정을 하는데요 (pi제어) 20ms시 각도제어 측정 후 속도제어를 측정하고싶으면 어떤식으로 코드를 짜야할까요? (인터럽트 처리 루틴질문입니다)
 
#include <avr/io.h>
#include "OK-2561BR.h"
 
#define  Tss    0.004			
#define  Kps    0.012			
#define  Kis    0.27				
 
#define Tsa     0.2
#define  Kpa    5				
#define  Kia    112.5
		
volatile unsigned char L_m1, L_count, L_count0 = 0;
volatile unsigned char R_m1, R_count, R_count0 = 0; 
volatile float L_Wr, L_Wref, Aref = 0;			
volatile float R_Wr, R_Wref;			
volatile float Wref = 500.;			
 
volatile unsigned char wheel = 'A';		// wheel selection('A'=all, 'L'=left, 'R'=right)
volatile float L_Werr, L_Werr0 = 0., L_PI = 0.;	// left motor speed error for PI control
volatile float R_Werr, R_Werr0 = 0., R_PI = 0.;	// right motor speed error for PI control
volatile float Aerr, Aerr0 = 0., A_PI = 0.;
volatile float PIconstants,APIconstants;			// speed PI control constant
 
volatile unsigned char RXD, RXD_status=0;	// USART1 receive temporary buffer and receive status
volatile unsigned char RXD_index, RXD_flag = 0;	// USART1 buffer index and receive data flag
volatile unsigned char RXD_buffer[8];		// RXD buffer(0xAA,YH,YL,PH,PL,RH,RL,0x55)
volatile unsigned char RXD_data[8];		// RXD data
 
float pitch;
 
ISR(USART1_RX_vect)				/* USART1 interrupt function */
{
	unsigned char i;
 
	RXD = UDR1;					// receive a character
 
	if((RXD_status == 1) && (RXD_index <= 7))	// receive data
	RXD_buffer[RXD_index++] = RXD;
 
	if((RXD_status == 0) && (RXD == 0xAA))	// 전송 시작 표시 ?
	{ RXD_status = 1;
		RXD_index = 0;
		RXD_buffer[RXD_index++] = RXD;
	}
	else if((RXD_index == 8) && (RXD == 0x55))	// 전송 종료 표시 ?
	{ RXD_status = 0;
		RXD_index = 0;
 
		RXD_flag = 1;				// if OK, store receive data
		for(i = 0; i <= 7; i++)
		RXD_data[i] = RXD_buffer[i];
	}
}
ISR(TIMER4_COMPA_vect)				/* Timer4 interrupt function(2.5ms period) */
{
	unsigned int PWM;				// PWM output data
	
	L_count = TCNT0;				// read TCNT0 for left motor
	R_count = TCNT1;				// read TCNT1 for right motor
	sei();                                        // global interrupt enable
 
	L_m1 = L_count - L_count0;			// calculate left motor speed
	L_count0 = L_count;
	L_Wr = (float)L_m1*60./52./Tss;
	if((PINC & 0x10) == 0x00) L_Wr *= -1.;
 
	R_m1 = R_count - R_count0;			// calculate right motor speed
	R_count0 = R_count;
	R_Wr = (float)R_m1*60./52./Tss;
	if((PINC & 0x20) == 0x00) R_Wr *= -1.;
 
	L_Werr = L_Wref - L_Wr;			// ***** speed PI controller of left motor
	L_PI = L_PI + PIconstants*L_Werr - Kps*L_Werr0;
	L_Werr0 = L_Werr;
 
	if(L_PI > 999.)       L_PI = 999.;		// PI control limiter
	else if(L_PI < -999.) L_PI = -999.;
 
	if(L_PI >= 0.)				// output PWM value for forward
	{ PWM = L_PI + 0.5;
		OCR3A = PWM;
		PORTC = (PORTC & 0xFC) | 0x02;
	}
	else						// output PWM value for backward
	{ PWM = -L_PI + 0.5;
		OCR3A = PWM;
		PORTC = (PORTC & 0xFC) | 0x01;
	}
 
	R_Werr = R_Wref - R_Wr;			// ***** speed PI controller of right motor
	R_PI = R_PI + PIconstants*R_Werr - Kps*R_Werr0;
	R_Werr0 = R_Werr;
 
	if(R_PI > 999.)       R_PI = 999.;		// PI control limiter
	else if(R_PI < -999.) R_PI = -999.;
        
	if(R_PI >= 0.)				// output PWM value for forward
	{ PWM = R_PI + 0.5;
		OCR3B = PWM;
		PORTC = (PORTC & 0xF3) | 0x08;
	}
	else						// output PWM value for backward
	{ PWM = -R_PI + 0.5;
		OCR3B = PWM;
		PORTC = (PORTC & 0xF3) | 0x04;
	}
	
	Aerr = Aref - pitch;			// 기울기 pi
	A_PI = A_PI + APIconstants*Aerr - Kpa*Aerr0;
	Aerr0 = Aerr;
 
	if(A_PI > 999.)       A_PI = 999.;		
	else if(A_PI < -999.) A_PI = -999.;
 
	if(A_PI >= 0.)				// output PWM value for forward
	{ PWM = A_PI + 0.5;
		OCR3A = PWM;
		OCR3B = PWM;
		PORTC = (PORTC & 0xFC) | 0x01;
		PORTC = (PORTC & 0xF3) | 0x04;
	}
	else						// output PWM value for backward
	{ PWM = -A_PI + 0.5;
		OCR3A = PWM;
		OCR3B = PWM;
		PORTC = (PORTC & 0xFC) | 0x02;
		PORTC = (PORTC & 0xF3) | 0x08;
	}
        
	asm volatile("PUSH R0");
	asm volatile("POP  R0");
}