向各位大侠求助：我用avr做两个步进电机控制一个悬挂物体运动，序调了好久也不理想！恳请

kingsong

#include<avr/io.h>
#include<math.h>
#include<avr/interrupt.h>
#include<avrbit.h>
#define uchar unsigned char
#define uint unsigned int
#define R_motor_state_0   0
#define R_motor_state_1   1
#define R_motor_state_2   2
#define R_motor_state_3   3
#define R_motor_state_4   4
#define R_motor_state_5   5
#define R_motor_state_6   6
#define R_motor_state_7   7
#define L_motor_state_0   0
#define L_motor_state_1   1
#define L_motor_state_2   2
#define L_motor_state_3   3
#define L_motor_state_4   4
#define L_motor_state_5   5
#define L_motor_state_6   6
#define L_motor_state_7   7                             
#define hl  26.7
int lm=100,rm=141,ly=100,ry=141,tl,tr,m1=100,m2=10,m3=100; //坐标原点初始状态
uint  NL,NR;    //电机移动步数
int l1,r1,j; //两线的长度
uchar  P_A;
int i=0;
uchar R_motor_state=0,L_motor_state=0,L_F_motor_state=0,R_F_motor_state=0;
uint R_count=0,L_count=0,R_motor_time=30,L_motor_time=30,R_flag=0,L_flag=0;

/*----函数声明-----*/
void xundian(uint x,uint y);
void huayuan(uint x,uint y,int R);
void R_motor();                          
void L_motor();
void R_F_motor();
void L_F_motor();
void delay(uint ms)
{
    uint i,j;
        for(i=0;i<ms;i++)
         {
           for(j=0;j<1141;j++);
     }
}

/*----主函数（程序入口）----*/
int main(void)
{

   
    DDRA=0XFF;  //  使能所在的PORTA端口
    PORTA=0XFF; //  设置所在管脚为输出
   
    TCCR0=0X02;  //8预分频1MHz
        TCNT0=0XE1;  // 设定时器初值30us
        TIMSK=0X01; // 开中断
        SREG|=BIT(7);
   
   
    xundian(20,30);
    delay(200);            //  延时约500ms
  }

void R_motor()                               //占用A端口
{
   switch(R_motor_state)
   {
      
      case R_motor_state_0:R_motor_state=R_motor_state_1;PORTA=(P_A&0X0F)|0XC0;P_A=PINA;break;
          case R_motor_state_1:R_motor_state=R_motor_state_2;PORTA=(P_A&0X0F)|0X40;P_A=PINA;break;
          case R_motor_state_2:R_motor_state=R_motor_state_3;PORTA=(P_A&0X0F)|0X60;P_A=PINA;break;
          case R_motor_state_3:R_motor_state=R_motor_state_4;PORTA=(P_A&0X0F)|0X20;P_A=PINA;break;
          case R_motor_state_4:R_motor_state=R_motor_state_5;PORTA=(P_A&0X0F)|0X30;P_A=PINA;break;
          case R_motor_state_5:R_motor_state=R_motor_state_6;PORTA=(P_A&0X0F)|0X10;P_A=PINA;break;
          case R_motor_state_6:R_motor_state=R_motor_state_7;PORTA=(P_A&0X0F)|0X90;P_A=PINA;break;
          case R_motor_state_7:R_motor_state=R_motor_state_0;PORTA=(P_A&0X0F)|0X80;P_A=PINA;break;      
   }
  
}
void L_motor()
{
   switch(L_motor_state)
   {
      case L_motor_state_0:L_motor_state=L_motor_state_1;PORTA=(P_A&0XF0)|0X08;P_A=PINA;break;
          case L_motor_state_1:L_motor_state=L_motor_state_2;PORTA=(P_A&0XF0)|0X09;P_A=PINA;break;
          case L_motor_state_2:L_motor_state=L_motor_state_3;PORTA=(P_A&0XF0)|0X01;P_A=PINA;break;
          case L_motor_state_3:L_motor_state=L_motor_state_4;PORTA=(P_A&0XF0)|0X03;P_A=PINA;break;
          case L_motor_state_4:L_motor_state=L_motor_state_5;PORTA=(P_A&0XF0)|0X02;P_A=PINA;break;
          case L_motor_state_5:L_motor_state=L_motor_state_6;PORTA=(P_A&0XF0)|0X06;P_A=PINA;break;
          case L_motor_state_6:L_motor_state=L_motor_state_7;PORTA=(P_A&0XF0)|0X04;P_A=PINA;break;
          case L_motor_state_7:L_motor_state=L_motor_state_0;PORTA=(P_A&0XF0)|0X0C;P_A=PINA;break;     
   }
   
}

void R_F_motor()
{
   switch(R_F_motor_state)
   {      
      case R_motor_state_0:R_F_motor_state=R_motor_state_1;PORTA=(P_A&0X0F)|0X80;P_A=PINA;break;
          case R_motor_state_1:R_F_motor_state=R_motor_state_2;PORTA=(P_A&0X0F)|0X90;P_A=PINA;break;
          case R_motor_state_2:R_F_motor_state=R_motor_state_3;PORTA=(P_A&0X0F)|0X10;P_A=PINA;break;
          case R_motor_state_3:R_F_motor_state=R_motor_state_4;PORTA=(P_A&0X0F)|0X30;P_A=PINA;break;
          case R_motor_state_4:R_F_motor_state=R_motor_state_5;PORTA=(P_A&0X0F)|0X20;P_A=PINA;break;
          case R_motor_state_5:R_F_motor_state=R_motor_state_6;PORTA=(P_A&0X0F)|0X60;P_A=PINA;break;
          case R_motor_state_6:R_F_motor_state=R_motor_state_7;PORTA=(P_A&0X0F)|0X40;P_A=PINA;break;
          case R_motor_state_7:R_F_motor_state=R_motor_state_0;PORTA=(P_A&0X0F)|0XC0;P_A=PINA;break;
               
   }
  
}
void L_F_motor()
{
   switch(L_F_motor_state)
   {
      case L_motor_state_0:L_F_motor_state=L_motor_state_1;PORTA=(P_A&0XF0)|0X0C;P_A=PINA;break;
          case L_motor_state_1:L_F_motor_state=L_motor_state_2;PORTA=(P_A&0XF0)|0X04;P_A=PINA;break;
          case L_motor_state_2:L_F_motor_state=L_motor_state_3;PORTA=(P_A&0XF0)|0X06;P_A=PINA;break;
          case L_motor_state_3:L_F_motor_state=L_motor_state_4;PORTA=(P_A&0XF0)|0X02;P_A=PINA;break;
          case L_motor_state_4:L_F_motor_state=L_motor_state_5;PORTA=(P_A&0XF0)|0X03;P_A=PINA;break;
          case L_motor_state_5:L_F_motor_state=L_motor_state_6;PORTA=(P_A&0XF0)|0X01;P_A=PINA;break;
          case L_motor_state_6:L_F_motor_state=L_motor_state_7;PORTA=(P_A&0XF0)|0X09;P_A=PINA;break;
          case L_motor_state_7:L_F_motor_state=L_motor_state_0;PORTA=(P_A&0XF0)|0X08;P_A=PINA;break;
         
   }
   
}

void xundian(uint x,uint y)
{
  ly=lm;
  ry=rm;
  m1=m1-y;
  m2=m2+x;
  m3=m3-x;
  lm=sqrt(pow(m1,2)+pow(m2,2));   //移动后到左滑轮的距离
  rm=sqrt(pow(m1,2)+pow(m3,2));   //移动后到右滑轮的距离
  l1=ly-lm;                       
  r1=ry-rm;
  if(ly>lm)
  tl=1;    //左电机正转收绳
  else
  tl=0;    //左电机反转放绳
  if(ry>rm)
  tr=1;    //右电机正转收绳
  else
  tr=0;    //右电机反转放绳
  NL=(uint)(fabs(l1)*hl);//左轮转动步数
  NR=(uint)(fabs(r1)*hl);//右轮转动步数

  while(NL>0||NR>0)
  {
   if( L_flag==1 && NL>0)
        {
             NL--;
             L_flag=0;
             if(tl==1)
             {
                L_motor();
             }
             else
             {
                L_F_motor();
             }  
        }
    if( R_flag==1 && NR>0)
        {
             NR--;
             R_flag=0;
              if(tr==1)
             {
               R_motor();
             }
             else
             {
                R_F_motor();
             }  
        }
  }
}
/*----定时器的中断服务函数----*/
ISR(TIMER0_OVF_vect)
{
             
        TCNT0=0XE1;//定时器初值
        if(NL>NR)
        {
          L_motor_time=30;          //为使两电机同时完成
          R_motor_time=30*NL/NR;
        }
        else
        {
          R_motor_time=30;
          L_motor_time=30*NR/NL;
        }
        R_count++;
        L_count++;
        if(R_count>R_motor_time)
        {
          R_flag=1;
          R_count=0;
        }
        if(L_count>L_motor_time)
        {
          L_flag=1;
          L_count=0;
        }

}