Mong được giúp đở pic18f4550 điều khiển rc servo.

[sky_103]

hiện em đang dùng Board PSC16 Điều Khiển Động Cơ RC Servo.dùng thạch anh 16M..ai giúp mình điều khiển với.
http://titans.com.vn/index.php?page=...chk=1&Itemid=1

code này do mình tự viết để test thử nhưng không được.mô phỏng thì đúng nhưng nạp vào thì ko chạy.
#include <18F4550.H>
#FUSES NOWDT, HS, NOPUT, NOPROTECT, NODEBUG, BROWNOUT, NOLVP, NOCPD, NOWRT
#use delay(clock = 16000000)
int16 i, goc1= 1400;
void hai_chan_truoc();
void goc_servo1();
void main()
{
set_tris_d(0x00);
enable_interrupts(global);
enable_interrupts(int_timer1);
setup_timer_1(T1_INTERNAL|T1_DIV_BY_4);
set_timer1(0);
while(1)
{
while(get_timer1()<=10);
hai_chan_truoc();
while (get_timer1()<= 20000);
set_timer1(0);
}
}
//==========================
void hai_chan_truoc()
{
goc_servo1();
}
//===========================
void goc_servo1()
{
i= get_timer1();
i= i+ goc1;
while (get_timer1() <= i ) //servo1
{output_high(pin_d1);} output_low(pin_d1);
}