Thắc mắc về timer?????

[ElecTel]

Chào các bạn !các bạn có thể xem giùm mình đoạn code này đã tối ưu chưa,mong sự giúp đở của mọi người!!!!!!
Đây là đoạn code sử dụng PID trong dò line

tại vì mình rất rất yếu về lập trình nên ko tự tin lắm khi tự mình viết code

mình sử dụng ngắt để tạo khoảng thời gian lấy mẩu
#int_timer0
void interrupt_timer0()
{

set_timer0(5);
++dem;
if(dem==3000)
{
dem=0;
};}
void main()
{
signed int setpoint=0,previous_error=0,error,control,read;
float output,derivative,integral=0;
enable_interrupts(int_timer0);
setup_timer_0(RTCC_INTERNAL | RTCC_DIV_2);
enable_interrupts(INT_RTCC);
set_timer0(5);
while(1)
{
if
(check_cross_line()==1)
{speed(70,70);
n++;} //nếu gặp vạch ngang thì tăng biến n
else
{
while(dem==3000) //dem 3000 tương ứng với 1 khoảng dt
{ //thực hiện việc lấy mẫu và tính toán Output
switch(sensor)
{
case 0b11111111 : read=0; break;
case 0b00011000 : read=0; break;
case 0b00010000 : read=-1; break;
case 0b00110000 : read=-2; break;//lech trai;
case 0b00100000 : read=-3; break;
case 0b01100000 : read=-4; break; //lech phai
case 0b01000000 : read=-5; break;
case 0b11000000 : read=-6; break;
case 0b10000000 : read=-7; break;
case 0b00001000 : read=1; break;
case 0b00001100 : read=2; break;//lech phai
case 0b00000100 : read=3; break;
case 0b00000110 : read=4; break;
case 0b00000010 : read=5; break;
case 0b00000011 : read=6; break;
case 0b00000001 : read=7; break;
case 0b00000000 : { if(n<=4) speed(100,40);if(n>4) speed(100,10);};break; //chổ này mình viết mình cũng cảm thấy khó hiểu ý tưởng của mình là khi không có vạch tương ứng với giá trj n mà làm...???!
}
error = setpoint - read;
integral = integral + (error*dem);
derivative = (error - previous_error)/dem;
output = (K*error) + (i*integral) + (d*derivative);
previous_error = error;
control=(signed int)(output);
if(control>max)
control=max;
if(control<-max)
control=-max;
}
if(control>0)
speed(max-control,max);
else
speed(max,max+control);}

}}