stepmotor.c

适用于单片机(51)和ARM的步进电机直线算法(两个步进电机组成的平面),且含有加减速算法.

/****************************************************************************
 
 x和y两个步进电机构成的一个平面上的直线函数,加减速直线函数. 使用Bresenham直线算法
 
 适用于lpc2114,CPU频率为55MHZ.
 该直线算法在51单片机上也能获得很好的效果
 
****************************************************************************/

typedef unsigned char  uint8;                   /* defined for unsigned 8-bits integer variable 	无符号8位整型变量  */
typedef signed   char  int8;                    /* defined for signed 8-bits integer variable		有符号8位整型变量  */
typedef unsigned short uint16;                  /* defined for unsigned 16-bits integer variable 	无符号16位整型变量 */
typedef signed   short int16;                   /* defined for signed 16-bits integer variable 		有符号16位整型变量 */
typedef unsigned int   uint32;                  /* defined for unsigned 32-bits integer variable 	无符号32位整型变量 */
typedef signed   int   int32;                   /* defined for signed 32-bits integer variable 		有符号32位整型变量 */
typedef float          fp32;                    /* single precision floating point variable (32bits) 单精度浮点数（32位长度） */
typedef double         fp64;                    /* double precision floating point variable (64bits) 双精度浮点数（64位长度） */

// I/O口定义
#define   XPU           0x00000400             //OUT P0.10口 X脉冲输出端 
#define   XDR           0x00000800             //OUT P0.11口 X方向控制端 - 0右1左 
#define   YPU           0x00001000             //OUT P0.12口 Y脉冲输出端 
#define   YDR           0x00002000             //OUT P0.13口 Y方向控制端 - 0上1下  

uint32 x0 = 0; //步进平面中,x的当前坐标
uint32 y0 = 0; //步进平面中,y的当前坐标

 //加/减速的相关参数,由系统参数中设置
uint8  dastep = 10;  //加减速时的计数步长(即N个脉冲才加减一次速),设置范围为1~100.


/****************************************************************************
* 名称：Delay_n(uint32 delayn)
* 功能：    延时
* 入口参数：delayn - 延时长度
* 出口参数：
* delay_n(1) = 约为0.108083us
****************************************************************************/
void Delay_n(uint32 delayn)
{ 
  for(; delayn > 0; delayn--);

}

/****************************************************************************
* 名称：move_x(uint8 dr,uint32 delay)
* 功能：x方向走一步
* 入口参数： dr   - 走线方向(1=正向,0=反向)
*           delay - 走线速度的延时
* 出口参数：x0(全局变量) - x的当前坐标值
* 
****************************************************************************/
void move_x(uint8 dr,uint32 delay)
{  uint8 xa;

    IO0CLR = XPU;    //输出低电平
     Delay_n(delay); //延时,为脉冲(低电平)宽度
    IO0SET = XPU;    //输出高电平
     Delay_n(delay); //延时,为脉冲(高电平)宽度
    //计算当前的坐标值
   if(dr)
    {
      x0++;
    }
   else  
    {   
      x0--;
    }

}
  
/****************************************************************************
* 名称：move_y(uint8 dr,uint32 delay)
* 功能：Y方向走一步
* 入口参数： dr   - 走线方向(1=正向,0=反向)
*           delay - 走线速度的延时
* 出口参数：y0(全局变量) - y的当前坐标值
* 
****************************************************************************/
void move_y(uint8 dr,uint32 delay)
{  uint8 xa;
    
    IO0CLR = YPU;    //输出低电平
     Delay_n(delay); //延时,为脉冲(低电平)宽度
    IO0SET = YPU;    //输出高电平
     Delay_n(delay); //延时,为脉冲(高电平)宽度
    //计算当前的坐标值
   if(dr)
    {
      y0++;
    }
   else  
    {   
      y0--;
    }
}

/****************************************************************************
* 名称： Line_To_xy(uint32 x1,uint32 y1,uint32 delayn) 
* 功能：x和y按指定的速度走一直线,从坐标(x0,y0)到(x1,y1)
* 入口参数：x1 - x的目标坐标
*           y1 - y的目标坐标
*           delayn - 走线速度
* 出口参数：
* //XY按设定的速度走一直线((x0,y0)-(x1,y1)))
****************************************************************************/
void Line_To_xy(uint32 x1,uint32 y1,uint32 delayn) 
{ int32  e,dx,dy;
  uint32 i,xdr,ydr;

   //确定走线的长度和方向(在这里直接设置步进电机的方向)
  if(x1 >= x0)
   {dx = x1 - x0; IO0CLR = XDR;xdr = 1;}   //X正向   0右1左
  else
   {dx = x0 - x1; IO0SET = XDR;xdr = 0;}   //X反向
  if(y1 >= y0)
   {dy = y1 - y0; IO0CLR = YDR;ydr = 1;}   //Y正向   0下1上
  else
   {dy = y0 - y1; IO0SET = YDR;ydr = 0;}   //Y反向

  
  if(dx >= dy) //x固定走dx步,y根据计算结束再决定是否走步进
  {
      e = -dx;
      for(i = 1; i <= dx; i++)
      { 
         e += 2 * dy;
           move_x(xdr,delayn); 
         if(e >= 0)  //判断Y是否走一步
          {
              move_y(ydr,delayn); 
              e = e - 2 * dx;
          }
      }
  }
  else   //y固定走dy步,x根据计算结束再决定是否步进
  {  
      e = -dy;
      for(i = 1; i <= dy; i++)
       {
          e = e + 2 * dx;
          move_y(ydr,delayn); 
          
          if(e >= 0)     //判断X是否走一步
            {
                move_x(xdr,delayn); 
                e = e - 2 * dy;
            }
       }
  }
}
/****************************************************************************
* 名称： Accel_To_xy(uint32 x1,uint32 y1,uint32 minaccel,uint32 maxdelay)
* 功能：x和y按加/减速走一条直线,从坐标(x0,y0)到(x1,y1). 加速度曲线为梯形
* 入口参数：x1 - x的目标坐标
*           y1 - y的目标坐标
*           minaccel - 加速前的初始速度(越大越慢)
*           maxdelay - 加速的最高速度(越小越快)
* 出口参数：

* 函数中的变量: 
*  acount -  加速器指针,即加速器在加速线上的当前位置.
*  alen   -  加速线的总长度  
*  shortlen -  1 = 其加速空间小于 "加速段 + 减速段" 的长度
*  accelsta -  0 = 速度不变; 0x5A = 加速; 0xA5 = 减速;
*  xcount   -  加速度计数器,到达加速度后,加/减一次速
*  dxdelay  -  加速/减速的步数
****************************************************************************/
void Accel_To_xy(uint32 x1,uint32 y1,uint32 minaccel,uint32 maxdelay)
{ int32  e,dx,dy,ndelay;
  uint32 i,acount=0,alen,dxdelay;
  uint8 xcount = 0,shortlen,accelsta = 0x5a,xdr,ydr;  //accelsta: 0=速度不变,5AH = 加速,A5H =减速;
  
  
   //确定走线的长度和方向(在这里直接设置步进电机的方向)
  if(x1 >= x0)
   {dx = x1 - x0; IO0CLR = XDR; xdr = 1;}   //X正向   0右1左
  else
   {dx = x0 - x1; IO0SET = XDR; xdr = 0;}   //X反向
  if(y1 >= y0)
   {dy = y1 - y0; IO0CLR = YDR; ydr = 1;}   //Y正向   0下1上
  else
   {dy = y0 - y1; IO0SET = YDR; ydr = 0;}   //Y反向

   
  alen = (dx + dy) / (2 * dastep);  //整个梯形曲线加减速曲线的总长度
  
  if(minaccel > maxdelay)
    {   dxdelay = minaccel - maxdelay; }  //加速/减速的步数
  else 
    {   dxdelay = 0; }     //无加减速  
    
  ndelay = minaccel;  //加减速的起始速度
  if(alen > dxdelay)  
    {shortlen = 0; alen = alen * 2;}
  else
    {shortlen = 1;}  //shortlen = 1,表示加速空间小于加,减速量
  

  if(dx >= dy) //x固定走dx步,y根据计算结束再决定是否走步进
  {
      e = -dx;
      for(i = 1; i <= dx; i++)
      { 
           xcount ++;       //由于加/减速的总步长为dx+dy,所以X和Y分别每走一步,都要进行加减速度处理
           if(xcount >= dastep) //x和y共同走了N步后进行一次加减速处理
              { acount ++;
                xcount = 0;
                 if(accelsta == 0x5a) ndelay --;       //加速
                 else if(accelsta == 0xa5) ndelay ++;  //减速
              }   
           e += 2 * dy;
           move_x(xdr,ndelay); 
         if(e >= 0)  //判断Y是否走一步
          {
              move_y(ydr,ndelay); 
             xcount++;       //由于加/减速的总步长为dx+dy,所以X和Y分别每走一步,都要进行加减速度处理
             if(xcount >= dastep)  //x和y共同走了N步后进行一次加减速处理
               { acount ++;
                 xcount = 0;
                 if(accelsta == 0x5a) ndelay --;       //加速
                 else if(accelsta == 0xa5) ndelay ++;  //减速
               }   
              e = e - 2 * dx;
          }
         if(ndelay < maxdelay) ndelay = maxdelay;
        
        if(shortlen) //加速空间小
         { if(accelsta == 0x5a)
              {//判断加速是否完成,以进入减速
                 if(acount >= alen)
                  {accelsta = 0xa5;alen=2 * alen;}
              }
            else if(accelsta == 0xa5)
              {//判断减速是否完成(减速完成后,以最低速运行剩下的线)
                 if(acount >= alen)
                  {accelsta = 0; ndelay = minaccel;}
              }
          
         }
        else  //加速空间大
         {
          if(accelsta == 0x5a)
             {//判断加速是否完成,以进入最高速
                 if(acount >= dxdelay) 
                 {  accelsta = 0; }
             }
           else if(accelsta == 0xa5)
             { //判断减速是否完成(减速完成后,以最低速运行剩下的线)
                if(acount >= alen)
                 {accelsta = 0; ndelay = minaccel;}
             }
            else
             {//判断最高速是否完成,以进入减速 
                if((acount + dxdelay) >= alen) 
                 {accelsta = 0xa5;}
             } 
          
         }
        
      }
  }
  else   //y固定走dy步,x根据计算结束再决定是否走步进
  {  
      e = -dy;
      for(i = 1; i <= dy; i++)
       {
         xcount++;       //由于加/减速的总步长为dx+dy,所以X和Y各走一步,都要计算加速度
         if(xcount >= dastep)//x和y共同走了N步后进行一次加减速处理
              { acount ++;
               xcount =0;
               if(accelsta == 0x5a) ndelay --;
                 else if(accelsta == 0xa5) ndelay ++;
              }   
           move_y(ydr,ndelay);  
         e = e + 2 * dx;
         if(e >= 0)     //判断X是否走一步
          {
            move_x(xdr,ndelay); 
            e = e - 2 * dy;
            xcount++;       //由于加/减速的总步长为dx+dy,所以X和Y各走一步,都要计算加速度
            if(xcount >= dastep)
              { acount ++;
                xcount = 0;
               if(accelsta == 0x5a) ndelay --;
                 else if(accelsta == 0xa5) ndelay ++;
              }   
            }
          if(ndelay < maxdelay) ndelay = maxdelay;
       
        if(shortlen) //加速空间小
         { if(accelsta == 0x5a)
              {//判断加速是否完成,以进入减速
                 if(acount >= alen) 
                  {accelsta = 0xa5;alen=2 * alen;}
              }
            else if(accelsta == 0xa5)
              {//判断减速是否完成(减速完成后,以最低速运行剩下的线)
                 if(acount >= alen) 
                  {accelsta = 0; ndelay = minaccel;}
              }
          
         }
        else  //加速空间大
         {
          if(accelsta == 0x5a)
             {//判断加速是否完成,以进入最高速
                 if(acount >= dxdelay) {accelsta = 0;}
             }
           else if(accelsta == 0xa5)
             { //判断减速是否完成(减速完成后,以最低速运行剩下的线)
                if(acount >= alen) {accelsta = 0; ndelay = minaccel;}
             }
            else
             {//判断最高速是否完成,以进入减速 
                if((acount + dxdelay) >= alen) {accelsta = 0xa5;}
             } 
          
         }
       }
  }
}