关于AVR单片机的问题6

各位高手，小弟正在基于AVR开发一款智能车，实现舵机超声波测距兼避障功能。遇到了一个问题：舵机只是单方向旋转，转动过程断断续续。请教其中的原因。谢谢大家。部分代码如下：
   while(1)
   {
   delay_nus (200);    

   start_SB();              
       if (CSB_data_lcd>400)
       {
         LCD_write_string(7,1,"Err!");  
       }
       else
       {
           ge =(CSB_data_lcd%100%10)+48;
           shi =(CSB_data_lcd%100/10)+48;
           bai =(CSB_data_lcd%1000/100)+48;
           qian =(CSB_data_lcd/1000)+48;
       
       LCD_write_char(7,1,qian);
       LCD_write_char(8,1,bai);
       LCD_write_char(9,1,shi);
       LCD_write_char(10,1,ge);
       LCD_write_string(12,1,"CM");

}

   if(CSB_data_lcd<20)
   {
       stop;
       delay_nms(10);
       k=1;
   }
   
if(k == 1)
{
if(s_1<1)
{
DUTY1 = 45;
S_L=CSB_data_lcd;
}
else if(s_1>=1&&s_1<2)
{
DUTY1 = 170;
S_R=CSB_data_lcd;
z=1;
}
else
{
DUTY1 = 110;
if(S_R>=15&&S_L<=15&&CSB_data_lcd<20)
{
left;
delay_nms(40);
stop;
}
else if(S_R<=15&&S_L>=15&&CSB_data_lcd<20)
{
right;
delay_nms(40);
stop;
}
else if(S_R<=15&&S_L<=15&&CSB_data_lcd<25&&z==1)
{
back;
delay_nms(200);
stop;
z=0;
}
else if(S_R>=15&&S_L>=15&&CSB_data_lcd<20)
{
right;
delay_nms(40);
stop;
}
else
{
k=0;
}
}
}
if(k==0)
{
   forward;
z=0;
}
if(s_1>2)
{
s_1 = 0;
ms_20 = 0;
}
}
}

#pragma interrupt_handler timer1_ovf_isr:9 //timer0 overflow
void timer1_ovf_isr(void)
{
  TCNT1H = 0xff;
TCNT1L = 0xF6;

t++;
 
if(t <= DUTY1)
  {

   PORTA |= (1<<PA0);
}
else
{
   PORTA &= ~(1<<PA0);
}

if(t >= 2000)
{
ms_20++;
t=0;
}

if(ms_20==50)
{
s_1++;
  ms_20=0;
}

}