基于arduino uno的delta并联机器人控制系统设计教程三

AimHigh

     教程三来教大家如何写出delta并联机器人中圆弧插补的代码
     圆弧插补和直线插补类似，对于任意一段平面圆弧上，我们需要找到圆弧起点到终点过程的插补点。如下图所示为圆弧插补图，我们选任意一圆心点O’，其坐标是O’(x0,y0,z0),A点为初始点，其坐标为（xm,ym,zm）,若以O为圆心，OA为半径，逆时针转b个角度，到达B点，实现以A到B逆时针圆弧插补。：[sf]
         [/sf]
        令邻近两插补点距离为h，且点都在圆上，如图AC距离为h。c为AC段圆弧对应的圆心角，因为h是定值，所以c为固定值。即：[sf]
          [/sf]
         其中r为圆弧半径为：[sf]

    [/sf]

     由上面分析得到平面圆弧插补流程图如下图所示，其中（xm,ym,zm）为起点坐标，(x0,y0,z0)为圆心坐标，angle为插补角度，h为插补精度，SN为插补方向（SN=0为顺时针，SN=1为逆时针），t为插补一次的时间间隔。[sf]

         [/sf]

          然后我们就可以在arduino IDE中写入相应的代码：

[sf]void chabu_circle(int xm,int ym,int zm,int x0,int y0,int z0,double angle,double h,int SN,int t)

{

  double x1,y1,z1,r,ang0,ang1,ang2;//ang0插补角度，ang1为变化角度,ang2为插补后的角度

  //n插补h的次数

  int n;

  x1=xm;

  y1=ym;

  z1=zm;

   r=sqrt((xm-x0)*(xm-x0)+(ym-y0)*(ym-y0));//半径

   ang0=2*asin(h/(2*r))*180/PI;//插补角度

   n=angle/ang0;

    if(SN==0)

    {

switch(gansmyongde(xm-x0,ym-y0,zm-z0,SN))

  {

    case 1:

    {

      if((xm-x0)==0)

      {

      ang1=90;

      ang2=90-angle;

      }

      else

      {

      ang1=atan((ym-y0)/(xm-x0))*180/PI;

     ang2=atan((ym-y0)/(xm-x0))*180/PI-angle;

      }

    }

    break;

    case 2:

    {

      ang1=180+atan((ym-y0)/(xm-x0))*180/PI;

      ang2=180+atan((ym-y0)/(xm-x0))*180/PI-angle;

      }

      break;

    case 3:

     {

      if((xm-x0)==0)

      {

      ang2=270-angle;

      ang1=270;

      }

      else

      {

       ang1=180+atan((ym-y0)/(xm-x0))*180/PI;

       ang2=180+atan((ym-y0)/(xm-x0))*180/PI-angle;

      }

    }

    break;

    case 4:

    {

      ang2=360+atan((ym-y0)/(xm-x0))*180/PI-angle;

      ang1=360+atan((ym-y0)/(xm-x0))*180/PI;

      }

      break;

    }

  }

    else

     {

switch(gansmyongde(xm-x0,ym-y0,zm-z0,SN))

  {

    case 1:

    {

     ang2=atan((ym-y0)/(xm-x0))*180/PI+angle;

     ang1=atan((ym-y0)/(xm-x0))*180/PI;

    }

    break;

    case 2:

    {

       if((xm-x0)==0)

      {

      ang2=90+angle;

      ang1=90;

      }

      else

      {

       ang2=180+atan((ym-y0)/(xm-x0))*180/PI+angle;

       ang1=180+atan((ym-y0)/(xm-x0))*180/PI;

      }

      }

      break;

    case 3:

     {

     ang2=180+atan((ym-y0)/(xm-x0))*180/PI+angle;

     ang1=180+atan((ym-y0)/(xm-x0))*180/PI;

    }

    break;

    case 4:

    {

      if((xm-x0)==0)

      {

      ang2=270+angle;

      ang1=270;

      }

      else

      {

       ang2=360-atan((ym-y0)/(xm-x0))*180/PI+angle;

       ang1=360-atan((ym-y0)/(xm-x0))*180/PI;

      }

     }

      break;

    }

  }

    double a1=nijie1(75,25,90,300,x1,y1,z1);

    double a2=nijie2(75,25,90,300,x1,y1,z1);

    double a3=nijie3(75,25,90,300,x1,y1,z1);

     myservo1.write(a1);

     myservo2.write(a2);

     myservo3.write(a3);

     delay(t);//微秒延迟函数delayMicroseconds()

  while(n>0)

  {

   if(SN==0)

   {

     ang1=ang1-ang0;

     x1=r*cos(ang1*PI/180)+x0;

     y1=r*sin(ang1*PI/180)+y0;

     a1=nijie1(75,25,90,300,x1,y1,z1);

     a2=nijie2(75,25,90,300,x1,y1,z1);

     a3=nijie3(75,25,90,300,x1,y1,z1);

     myservo1.write(a1);

     myservo2.write(a2);

     myservo3.write(a3);

     delay(t);//微秒延迟函数delayMicroseconds()

    }

    else

    {

     ang1=ang1+ang0;

     x1=r*cos(ang1*PI/180)+x0;

     y1=r*sin(ang1*PI/180)+y0;

     a1=nijie1(75,25,90,300,x1,y1,z1);

     a2=nijie2(75,25,90,300,x1,y1,z1);

     a3=nijie3(75,25,90,300,x1,y1,z1);

     myservo1.write(a1);

     myservo2.write(a2);

     myservo3.write(a3);

     delay(t);//微秒延迟函数delayMicroseconds()

      }

     n=n-1;

     }

      x1=r*cos(ang2*PI/180)+x0;

      y1=r*sin(ang2*PI/180)+y0;

      a1=nijie1(75,25,90,300,x1,y1,z1);

      a2=nijie2(75,25,90,300,x1,y1,z1);

      a3=nijie3(75,25,90,300,x1,y1,z1);

      myservo1.write(a1);

      myservo2.write(a2);

      myservo3.write(a3);

      delay(t);//微秒延迟函数delayMicroseconds()

    }

//xm，ym象限判断函数，根据插补方向SN和xm，ym的值判断?
int gansmyongde(int xm,int ym,int zm,int SN)
{
  int i=0;
  if(SN==0)
  {
    if((xm>=0)&&(ym>0))
        i=1;
    if((xm<0)&&(ym>=0))
        i=2;
    if((xm<=0)&&(ym<0))
        i=3;
    if((xm>0)&&(ym<=0))
        i=4;
    }
  else if(SN==1)
  {
    if((xm>0)&&(ym>=0))
        i=1;
    if((xm<=0)&&(ym>0))
        i=2;
    if((xm<0)&&(ym<=0))
        i=3;
    if((xm>=0)&&(ym<0))
        i=4;
    }
    return i;
  }[/sf]