Điều khiển động cơ servo

[tungoctran]

chào các anh các chị
Em đang dùng 4 dc servo MG946R để điểu khiển cánh tay máy như clip bên dưới .còn mấy ngày nữa báo cáo rồi mà chưa chạy mong anh chị giúp em
em dung con 16f877a qua mach nap big kit2 va driver starter1 [mua ở thiên minh]
đây là đoạn code: tan so 16MHZ
#include <16f877A.h>
#include <def_877a.h>
#device *=16 ADC=8
#FUSES NOWDT, HS, NOPUT, NOPROTECT, NODEBUG, BROWNOUT, NOLVP, NOCPD, NOWRT
#use rs232(baud=9600,Xmit=PIN_C6,rcv=PIN_C7)
#use rs232
#use msdelay(clock=160000000)
#INT_RA
#INT_RB;
#INT_RC;
#INT_RD;
char dem;
char value,value1,value2,value3;
#INT_TIMER0
Void set_servo() {
set_timer0(164);
dem++; //bien dem tang thêm 1 giá tri moi 0,1ms
if (dem >= 200) // 200*0,1=20ms (xung 50Hz)
{
dem=0; //
output_high(PIN_A1);
output_high(PIN_B1);
output_high(PIN_C1);
output_high(PIN_D1);
msdelay(5000);
}
if (dem >= value)output_LOW(PIN_A1);
if (dem >= value1)output_LOW(PIN_B1);
if (dem >= value2)output_LOW(PIN_C1);
if (dem >= value3)output_LOW(PIN_D1);
msdelay(5000);
}


void main()
{
set_tris_a(0);
OUTPUT_A(0xff);
set_tris_b(0x00);
OUTPUT_B(0xff;
set_tris_c(0x00);
OUTPUT_C(0xff);
set_tris_d(0x00);
OUTPUT_D(0xff);
enable_interrupts(int_timer0); //cho phép ng?t timer0
enable_interrupts(global);
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_4); //chon t?n s? cho timer0 t? clock chính (16MHz)
set_timer0(156); //cài ??t timer ?? timer0 tràn m?i 0,1ms
while(true) //vòng lap
{
value=12;
value1=12;
value2=12;
value3=13;
}
}