查看: 1979|回复: 0

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

[复制链接]
  • TA的每日心情

    2020-2-9 17:38
  • 签到天数: 330 天

    [LV.8]以坛为家I

    发表于 2018-9-15 10:43 | 显示全部楼层 |阅读模式
    本帖最后由 AimHigh 于 2018-9-15 11:06 编辑

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

    3.JPG     [/sf]
         由上面分析得到平面圆弧插补流程图如下图所示,其中(xm,ym,zm)为起点坐标,(x0,y0,z0)为圆心坐标,angle为插补角度,h为插补精度,SN为插补方向(SN=0为顺时针,SN=1为逆时针),t为插补一次的时间间隔。[sf]
             4.JPG 5.JPG [/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]
    您需要登录后才可以回帖 登录 | 立即注册

    本版积分规则

    热门推荐

    《Color Unit免费申请试用》
    《Color Unit免费申请试用
    COLOR 是一款颜色识别 Unit,其内部集成TCS3472彩色光数字转换器,能够将其检测到颜色
    【干货分享】mega2560原理图PCB图纸altium designer18
    【干货分享】mega2560原理
    分享一下mega2560的板子 AD版本 **** 本内容被作者隐藏 **** ergo
    求助帖,ESP01S介入WS2812B,无法让灯
    求助帖,ESP01S介入WS2812
    看抖音暴改车间着迷,但是无法调试出氛围灯,新手,有没有大神帮忙解决一下,感谢 上
    完美解决同步开关状态 ,小爱同学+app+本地按键控制状态...
    完美解决同步开关状态 ,
    你有没有发现app上面的按键在操作完毕后你就不知道开关的状态了;或者小爱操作完毕后
    DW542驱动器控制42步进电机
    DW542驱动器控制42步进电
    求一个范例,或者现成的代码,控制42步进电机,实现控制转台转动,要求可以控制转台转
    Copyright   ©2015-2016  Arduino中文社区  Powered by©Discuz!   
    快速回复 返回顶部 返回列表