Giúp em Test cái mạch đọc encoder

[hustk53]

Chào các bro !
Mình mới học Dspic,tập tành viết một chương trình nhỏ để điều khiển động cơ. Nhưng mình viết mãi cái hàm nhận encoder mà nó không chạy.
Các bro giúp em test bài này với. Em đang cần hoàn thành gấp mà giờ cứ bế tắc thế này, chán quá. Mong các bro giúp em với
code và mạch em đính kèm trong file. Còn đây là đoạn mà em viết cho encoder.

void __builtin_write_OSCCONL(unsigned char value);
void __builtin_write_OSCCONH(unsigned char value); // hàm để mở khóa OSCCON

void init_Ports(void) // dinh ngia cac cong
{
PWM_TRIS=0x0000; //dat cac cong PWM la ngo ra chan RB
TRISC=0x048F; //dat cac chan C3-C0 la ngo vao
PWM_LAT=0x0000; //xoa thanh dem cua pwm
_IOLOCK=0; // Mở khóa để ghi vào thanh ghi RPINR
RPINR14=0x1918; //dùng chân RP25 và RP24 để nhận QEA và QEB
RPINR15=0x0015; // RP21 cho chân index
}

void init_encoder(void)
{
AD1PCFGL=0x001E; // chọn các chân vào là số
QEICON=0x0604; // mode x4 ,reset=INDEX xung
DFLTCON=0x00E0; // có loc dau ra voi tan so chia 32
POSCNT=0x0000; // xóa thanh ghi chua
MAXCNT=0x00FF; // tao gia tri so sanh
_QEIIF=0; // xoa co ngat
_QEIIE=1; // cho phep ngat
}
// viet ngat
void _ISR _QEIInterrupt(void)
{
P1DC1=POSCNT; //test thử để xem chương trình có chạy không
P1DC2 = 0x0000;
_LATA9=1; // báo LED nếu ngắt xảy ra
_QEIIF=0;
}