查看: 4633|回复: 42

[项目] 基于arduino的delta并联机械手

[复制链接]
  • TA的每日心情
    慵懒
    2019-9-18 16:39
  • 签到天数: 326 天

    [LV.8]以坛为家I

    发表于 2019-1-6 17:06 | 显示全部楼层 |阅读模式
          delta并联机械手现在广泛应用于工业,因为资金有限,用舵机做的,底座这些也是用的实验室抽屉打孔固定,已完工。                                                                                                                                                                                              微信图片_20190106160023.jpg
           首先说一下我的整体方案设计见图,它包括三个模块分别是舵机执行模块、电磁铁执行模块和串口通讯模块。其中舵机模块也是最重要的模块,需要三个舵机实现插补完成在空间走任意线段,定点搬运打孔等功能。
                                                                                                                 微信图片_20190106170743.png
    游客,如果您要查看本帖隐藏内容请回复

                                                                                                        微信图片_20190106161008.png
        建立如图所示的动、静两个坐标系,静坐标系O-XYZ的原点O位于静平台的几何中心,即等边三角形A1A2A3的几何中心,Z轴垂直于静平台,OX垂直于A1A3。动平台上建立O’-X’Y’Z’坐标系,O’为动平台的几何中心,Z’轴垂直于动平台,O’X’垂直于C1C3。其中AiBi=L,CiBi=l。设定静平台的外接圆半径为R ,即OAI=R;设定动平台外接圆半径为r ,即O’C’I=r。机构运动时主动臂与垂直方向之间的夹角为θi,其中i= 1,2,3。机器人通过三个分支链将上下平台连接起来,驱动臂在电机的驱动下作一定角度的反复摆动,再通过平行四边形闭环和转动副使动平台作平移运动。然后需要进行运动学分析,求逆解,位置逆解就是已知机器人动平台在静平台坐标系O-XYZ中的位置,求解主动臂AiBi与静平台平面之间的夹角(即(90°-θi))。需要点数学基础,计算得到3个位置逆解如下:
                                微信图片_20190106161243.png 微信图片_20190106161307.png 微信图片_20190106161321.png
    再将运动学写成代码即可,如下所示为逆解代码:

    kittenblock中小学创客名师推荐的图形化编程软件

    double nijie1(double R,double r,double L,double l,double x,double y,double z)
    {
      double K,M,N;
      K=((-(x*x+y*y+z*z)+(y+sqrt(3)*x)*(R-r)-(R-r)*(R-r)-L*L+l*l)/L+2*z);
      M=-2*(2*(R-r)-y-sqrt(3)*x);
      N=((-(x*x+y*y+z*z)+(y+sqrt(3)*x)*(R-r)-(R-r)*(R-r)-L*L+l*l)/L-2*z);
      double a=90-2*atan((-M-sqrt(M*M-4*K*N))/(2*K))*180/PI+30;
      return(a);//返回角度
    }

        为了使delta并联机械手末端执行器走出自己需要的轨迹,则需要加入插补算法,所以需要写好直线插补和空间圆弧插补算法。
    直线插补假设机器人动平台在空间坐标系中通过直线插补从A点运动到B点,插补精度为h,插补点时间间隔t。A点坐标为(xm,ym,zm),B点坐标为(xe,ye,ze)。C点为插补过程点(x1,y1,z1)。如图为直线插补图
    微信图片_20190106161825.png
    很简单的就可以求出BC直线段的距离,为了简化表述,做了流程图,插补具体流程图如下所示
                                                                                                    微信图片_20190106161950.png
    根据流程图将空间直线插补代码写入即可,如下所示:

    kittenblock中小学创客名师推荐的图形化编程软件

    void chabu_line(double xm,double ym,double zm,double xe,double ye,double ze,double h,int t)
    {
      double x1,y1,z1,s;
      x1=xm;
      y1=ym;
      z1=zm;
      s=1;
         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(s>0)
      {
        s=sqrt((xe-x1)*(xe-x1)+(ye-y1)*(ye-y1)+(ze-z1)*(ze-z1));
        if(s>h)
        {
        x1=h*(xe-x1)/s+x1;
        y1=h*(ye-y1)/s+y1;
        z1=h*(ze-z1)/s+z1;
        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
        {
          a1=nijie1(75,25,90,300,xe,ye,ze);
          a2=nijie2(75,25,90,300,xe,ye,ze);
          a3=nijie3(75,25,90,300,xe,ye,ze);
          myservo1.write(a1);
          myservo2.write(a2);
          myservo3.write(a3);
          s=0;
          }
      }
    }


    圆弧插补:圆弧插补和直线插补类似,对于任意一段平面圆弧上,我们需要找到圆弧起点到终点过程的插补点。如图所示为圆弧插补图,我们选任意一圆心点O’,其坐标是O’(x0,y0,z0),A点为初始点,其坐标为(xm,ym,zm),若以O为圆心,OA为半径,逆时针转b个角度,到达B点,实现以A到B逆时针圆弧插补。
    微信图片_20190106162258.png
          其中(xm,ym,zm)为起点坐标,(x0,y0,z0)为圆心坐标,angle为插补角度,h为插补精度,SN为插补方向(SN=0为顺时针,SN=1为逆时针),t为插补一次的时间间隔。
                                                                                                                    微信图片_20190106162458.png
             同样的,跟直线插补一样,根据流程图将空间圆弧插补写好即可。
           通过上述功能就可以实现走空间任意线段,其运动速度可以慢/中/快(改变延时即可),拍了个简单的视频如下(直线比较简单,就没拍了)
               还需要实现搬运或打孔,只需要末端执行器上装个吸盘或者电机孔就行,我在实验室找了一个小的吸盘装上,控制原理很简单
                                                                                                     微信图片_20190106163518.png 微信图片_20190106164356.png
                  因为吸盘是12V的,所以要用继电器,到时继电器与吸盘之间记得外接12V的电源,然后常开的就行,关于代码怎么写,具体看你继电器的使用了,我的是送高电平就行,拍了简单的搬运视频如下
           关于总的电路怎么接,很简单,就是arduino与3舵机相连,舵机接法也比较简单,如下图,照着接就行,继电器接法就看上面说的就好。

    微信图片_20190106164704.png   
           关于机械结构的话,底座和靠背都是用的抽屉做的,起固定作用,主动臂与被动臂某宝买的,报尺寸就行。静平台的话就需要自己设计一下,要考虑主动臂在旋转过程中与其干涉的问题,还要特别注意三个电机的安装位置,因为只有保证电机的安装位置在正三角形的三个顶点上,机器人运动时精度才能保证。同时还要注意静平台以机架之间的安装孔的设计。
                                                                                                             微信图片_20190106165740.png
              动平台就需要配合臂,实物图如下
                                                                                                                 微信图片_20190106170039.png

              程序源码我上传了百度云,需要的可以自行下载:

    kittenblock中小学创客名师推荐的图形化编程软件

    #include 
    
    Servo myservo1,myservo2,myservo3; 
    String comdata = "";
    int relayPin =3;
    int numdata[9] = {0}, mark = 0,mark1 = 0;//加了一个指针,来判断是否有"-"负号存在。
    double xm=0,ym=0,zm=-290;
    double xe,ye,ze;
    void setup()
    {
      pinMode(relayPin, OUTPUT);
       myservo1.attach(11);// attaches the servo on pin 9 to the servo object
       myservo2.attach(10);
       myservo3.attach(12);
      Serial.begin(9600);
    }
    
    void loop()
    {
      
    //实验用(圆弧)
    //digitalWrite(relayPin, HIGH);
    //myservo1.write(60);
    //    myservo2.write(60);
    //    myservo3.write(45);、
    digitalWrite(relayPin, HIGH);
    chabu_circle(50,0,-250,0,0,-250,360,1,0,10);
    delay(2000);
    
    digitalWrite(relayPin, LOW);
    chabu_circle(50,0,-250,0,0,-250,360,1,0,5);
    delay(2000);
    digitalWrite(relayPin, HIGH);
    chabu_circle(50,0,-250,0,0,-250,360,1,0,1);
    delay(2000);
    digitalWrite(relayPin, LOW);
    //实验用(直线)确定h和t最佳取值
    ////h=1,t=10
    //delay(5000);
    //chabu_line(0,70,-290,0,-70,-290,1,10);
    //delay(5000);
    //chabu_line(0,-70,-290,0,70,-290,1,10);
    //h=1,t=5
    //delay(5000);
    //chabu_line(0,70,-290,0,-70,-290,1,5);
    //delay(5000);
    //chabu_line(0,-70,-290,0,70,-290,1,5);
    ////h=1,t=1
    //delay(5000);
    //chabu_line(0,70,-290,0,-70,-290,1,1);
    //delay(5000);
    //chabu_line(0,70,-290,0,-70,-290,1,1);
    
    
    //串口通讯1,实现一对一地串口输入一组数据,动平台动一次
    // myservo1.write(nijie1(75,25,90,300,xm,ym,zm));
    // myservo2.write(nijie2(75,25,90,300,xm,ym,zm));
    // myservo3.write(nijie3(75,25,90,300,xm,ym,zm));
    //  int j = 0;
    //  while (Serial.available() > 0)
    //  {
    //    comdata += char(Serial.read());
    //    delay(2);
    //    mark = 1;
    //  }
    //  if(mark == 1)
    //  {
    //    Serial.println(comdata);
    //    Serial.println(comdata.length());
    //    
    //    for(int i=0;i<comdata.length()-1; i++)
    //    {
    //   if(comdata == ',') 
    //  {
    //     ++j;//
    //     mark1=0;//读取到','时,将mark1恢复到0值,便于下个字符是"-"时,赋予"1" 
    //   }
    //  else 
    //    {
    //      if(comdata=='-')
    //    {
    //      mark1=1;//判断是否有负号"-"
    //    }
    //    else 
    //    {
    //    if(mark1==1)
    //     {
    //      numdata[j]=numdata[j]*10 - (comdata - '0');
    //     }
    //     else
    //     {
    //      numdata[j]=numdata[j] * 10 + (comdata - '0');
    //     }
    //    }
    //     }
    //   }
    //    comdata = String("");
    //    if(numdata[0]==0)
    //    {
    //      int xe=numdata[1];
    //      int ye=numdata[2];
    //      int ze=numdata[3];
    //      int h=numdata[4];
    //      int t=numdata[5];
    //      chabu_line(xm,ym,zm,xe,ye,ze,h,t);
    //      delay(2);
    //      xm=xe;
    //      ym=ye;
    //      zm=ze;
    //      }
    //    else if(numdata[0]==1)
    //    {
    //      double x0=numdata[1];
    //      double y0=numdata[2];
    //      double z0=numdata[3];
    //      double angle=numdata[4];
    //      double h=numdata[5];
    //      int SN=numdata[6];
    //      int t=numdata[7];
    //      chabu_circle(xm,ym,zm,x0,y0,z0,angle,h,SN,t);
    //      delay(2);
    //      xm=xe;
    //      ym=ye;
    //      zm=ze;
    //      }
    //    for(int i=0;i<8;i++)
    //    {
    //      Serial.print(" = ");
    //      Serial.println(numdata);
    //      numdata = 0;
    //    }
    //    mark = 0;
    //  }
    }
    
    //逆解计算函数
    double nijie1(double R,double r,double L,double l,double x,double y,double z)
    {
      double K,M,N;
      K=((-(x*x+y*y+z*z)+(y+sqrt(3)*x)*(R-r)-(R-r)*(R-r)-L*L+l*l)/L+2*z);
      M=-2*(2*(R-r)-y-sqrt(3)*x);
      N=((-(x*x+y*y+z*z)+(y+sqrt(3)*x)*(R-r)-(R-r)*(R-r)-L*L+l*l)/L-2*z);
      double a=90-2*atan((-M-sqrt(M*M-4*K*N))/(2*K))*180/PI+30;
      return(a);//返回角度
    }
    
    double nijie2(double R,double r,double L,double l,double x,double y,double z)
    {
      double K,M,N;
      K=((-(x*x+y*y+z*z)-(-y+sqrt(3)*x)*(R-r)-(R-r)*(R-r)-L*L+l*l)/L+2*z);
      M=-2*(2*(R-r)-y+sqrt(3)*x);
      N=((-(x*x+y*y+z*z)-(-y+sqrt(3)*x)*(R-r)-(R-r)*(R-r)-L*L+l*l)/L-2*z);
      double a=90-2*atan((-M-sqrt(M*M-4*K*N))/(2*K))*180/PI+30;
      return(a);//返回角度
    }
    
    double nijie3(double R,double r,double L,double l,double x,double y,double z)
    {
      double K,M,N;  
    K=((-(x*x+y*y+z*z)-2*y*(R-r)-(R-r)*(R-r)-L*L+l*l)/(2*L)+z);
    M=-2*(R-r+y);
    N=((-(x*x+y*y+z*z)-2*y*(R-r)-(R-r)*(R-r)-L*L+l*l)/(2*L)-z);
      double a=90-2*atan((-M-sqrt(M*M-4*K*N))/(2*K))*180/PI+30;
      return(a);//返回角度
    }
    
    //直线插补函数
    /*此函数是实现在空间直线插补功能   */
    void chabu_line(double xm,double ym,double zm,double xe,double ye,double ze,double h,int t)
    {
      double x1,y1,z1,s;
      x1=xm;
      y1=ym;
      z1=zm;
      s=1;
         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(s>0)
      {
        s=sqrt((xe-x1)*(xe-x1)+(ye-y1)*(ye-y1)+(ze-z1)*(ze-z1));
        if(s>h)
        {
        x1=h*(xe-x1)/s+x1;
        y1=h*(ye-y1)/s+y1;
        z1=h*(ze-z1)/s+z1;
        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
        {
          a1=nijie1(75,25,90,300,xe,ye,ze);
          a2=nijie2(75,25,90,300,xe,ye,ze);
          a3=nijie3(75,25,90,300,xe,ye,ze);
          myservo1.write(a1);
          myservo2.write(a2);
          myservo3.write(a3);
          s=0;
          }
      }
    }
    
    //平面圆弧插补
    void chabu_circle(double xm,double ym,double zm,double x0,double y0,double z0,double angle,int h,int SN,int t)
    {
      if(zm==z0)
         chabu_circle_xy(xm,ym,zm,x0, y0,z0,angle,h,SN,t);
       else
       {
        if(xm==x0)
        chabu_circle_yz(xm,ym,zm,x0, y0,z0,angle,h,SN,t);
        else
        chabu_circle_xz(xm,ym,zm,x0, y0,z0,angle,h,SN,t);
        }
     }
    //xoy平面或与xoy平面平面圆弧插补
    void chabu_circle_xy(double xm,double ym,double zm,double x0,double y0,double z0,double angle,int h,int SN,int t)
    {
       //ang0插补角度,ang1为变化角度,ang2为插补后的角度
      double r,ang0,ang1,ang2;
      //n插补h的次数
      int n;
      xe=xm;
      ye=ym;
      ze=zm;
       r=sqrt((xm-x0)*(xm-x0)+(ym-y0)*(zm-z0));//半径
       ang0=2*asin(h/(2*r))*180/PI;//插补角度
       n=angle/ang0;
        if(SN==0)
        {
    switch(gansmyongde_xy(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_xy(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,xe,ye,ze);
        double a2=nijie2(75,25,90,300,xe,ye,ze);
        double a3=nijie3(75,25,90,300,xe,ye,ze);
         myservo1.write(a1);
         myservo2.write(a2);
         myservo3.write(a3);
         delay(t);//微秒延迟函数delayMicroseconds()
      while(n>0)
      {
       if(SN==0)
       {
         ang1=ang1-ang0;
         xe=r*cos(ang1*PI/180)+x0;
         ye=r*sin(ang1*PI/180)+y0;
         a1=nijie1(75,25,90,300,xe,ye,ze);
         a2=nijie2(75,25,90,300,xe,ye,ze);
         a3=nijie3(75,25,90,300,xe,ye,ze);
         myservo1.write(a1);
         myservo2.write(a2);
         myservo3.write(a3);
         delay(t);//微秒延迟函数delayMicroseconds()
        }
        else
        {
         ang1=ang1+ang0;
         xe=r*cos(ang1*PI/180)+x0;
         ye=r*sin(ang1*PI/180)+y0;
         a1=nijie1(75,25,90,300,xe,ye,ze);
         a2=nijie2(75,25,90,300,xe,ye,ze);
         a3=nijie3(75,25,90,300,xe,ye,ze);
         myservo1.write(a1);
         myservo2.write(a2);
         myservo3.write(a3);
         delay(t);//微秒延迟函数delayMicroseconds()
          }
         n=n-1;
         }
          xe=r*cos(ang2*PI/180)+x0;
          ye=r*sin(ang2*PI/180)+y0;
          a1=nijie1(75,25,90,300,xe,ye,ze);
          a2=nijie2(75,25,90,300,xe,ye,ze);
          a3=nijie3(75,25,90,300,xe,ye,ze);
          myservo1.write(a1);
          myservo2.write(a2);
          myservo3.write(a3);
          delay(t);//微秒延迟函数delayMicroseconds()
        }
    
    //yoz平面或与yoz平行地平面圆弧插补
    void chabu_circle_yz(double xm,double ym,double zm,double x0,double y0,double z0,double angle,int h,int SN,int t)
    {
       //ang0插补角度,ang1为变化角度,ang2为插补后的角度
      double r,ang0,ang1,ang2;
      //n插补h的次数
      int n;
      xe=xm;
      ye=ym;
      ze=zm;
       r=sqrt((xm-x0)*(xm-x0)+(ym-y0)*(ym-y0)+(zm-z0)*(zm-z0));//半径
       ang0=2*asin(h/(2*r))*180/PI;//插补角度(由h来确定插补值)
       n=angle/ang0;
        if(SN==0)
        {
    switch(gansmyongde_yz(xm-x0,ym-y0,zm-z0,SN))
      {
        case 1:
        {
          if((ym-y0)==0)
          {
          ang1=90;
          ang2=90-angle;
          }
          else
          {
          ang1=atan((zm-z0)/(ym-y0))*180/PI;
         ang2=atan((zm-z0)/(ym-y0))*180/PI;-angle;
          }
        }
        break;
        case 2:
        {
          ang1=180+atan((zm-z0)/(ym-y0))*180/PI;
          ang2=180+atan((zm-z0)/(ym-y0))*180/PI-angle;
          }
          break;
        case 3:
         {
          if((ym-y0)==0)
          {
          ang2=270-angle;
          ang1=270;
          }
          else
          {
           ang1=180+atan((zm-z0)/(ym-y0))*180/PI;
           ang2=180+atan((zm-z0)/(ym-y0))*180/PI-angle;
          }
        }
        break;
        case 4:
        {
          ang2=360+atan((zm-z0)/(ym-y0))*180/PI-angle;
          ang1=360+atan((zm-z0)/(ym-y0))*180/PI;
          }
          break;
        }
      }
        else
         {
    switch(gansmyongde_yz(xm-x0,ym-y0,zm-z0,SN))
      {
        case 1:
        {
         ang2=atan((zm-z0)/(ym-y0))*180/PI+angle;
         ang1=atan((zm-z0)/(ym-y0))*180/PI;
        }
        break;
        case 2:
        {
           if((ym-y0)==0)
          {
          ang2=90+angle;
          ang1=90;
          }
          else
          {
           ang2=180+atan((zm-z0)/(ym-y0))*180/PI+angle;
           ang1=180+atan((zm-z0)/(ym-y0))*180/PI;
          }
          }
          break;
        case 3:
         {
         ang2=180+atan((zm-z0)/(ym-y0))*180/PI+angle;
         ang1=180+atan((zm-z0)/(ym-y0))*180/PI;
        }
        break;
        case 4:
        {
          if((ym-y0)==0)
          {
          ang2=270+angle;
          ang1=270;
          }
          else
          {
           ang2=360-atan((zm-z0)/(ym-y0))*180/PI+angle;
           ang1=360-atan((zm-z0)/(ym-y0))*180/PI;
          }
         }
          break;
        }
      }
        double a1=nijie1(75,25,90,300,xe,ye,ze);
        double a2=nijie2(75,25,90,300,xe,ye,ze);
        double a3=nijie3(75,25,90,300,xe,ye,ze);
         myservo1.write(a1);
         myservo2.write(a2);
         myservo3.write(a3);
         delay(t);//微秒延迟函数delayMicroseconds()
      while(n>0)
      {
       if(SN==0)
       {
         ang1=ang1-ang0;
         ye=r*cos(ang1*PI/180)+y0;
         ze=r*sin(ang1*PI/180)+z0;
         a1=nijie1(75,25,90,300,xe,ye,ze);
         a2=nijie2(75,25,90,300,xe,ye,ze);
         a3=nijie3(75,25,90,300,xe,ye,ze);
         myservo1.write(a1);
         myservo2.write(a2);
         myservo3.write(a3);
         delay(t);//微秒延迟函数delayMicroseconds()
        }
        else
        {
         ang1=ang1+ang0;
         ye=r*cos(ang1*PI/180)+y0;
         ze=r*sin(ang1*PI/180)+z0;
         a1=nijie1(75,25,90,300,xe,ye,ze);
         a2=nijie2(75,25,90,300,xe,ye,ze);
         a3=nijie3(75,25,90,300,xe,ye,ze);
         myservo1.write(a1);
         myservo2.write(a2);
         myservo3.write(a3);
         delay(t);//微秒延迟函数delayMicroseconds()
          }
         n=n-1;
         }
          ye=r*cos(ang2*PI/180)+y0;
          ze=r*sin(ang2*PI/180)+z0;
          a1=nijie1(75,25,90,300,xe,ye,ze);
          a2=nijie2(75,25,90,300,xe,ye,ze);
          a3=nijie3(75,25,90,300,xe,ye,ze);
          myservo1.write(a1);
          myservo2.write(a2);
          myservo3.write(a3);
          delay(t);//微秒延迟函数delayMicroseconds()
        }
    
    //xoz平面或与xoz平面平面圆弧插补
    void chabu_circle_xz(double xm,double ym,double zm,double x0,double y0,double z0,double angle,int h,int SN,int t)
    {
       //ang0插补角度,ang1为变化角度,ang2为插补后的角度
      double r,ang0,ang1,ang2;
      //n插补h的次数
      int n;
      xe=xm;
      ye=ym;
      ze=zm;
       r=sqrt((xm-x0)*(xm-x0)+(ym-y0)*(zm-z0)+(zm-z0)*(zm-z0));//半径
       ang0=2*asin(h/(2*r))*180/PI;//插补角度
       n=angle/ang0;
        if(SN==0)
        {
    switch(gansmyongde_xz(xm-x0,ym-y0,zm-z0,SN))
      {
        case 1:
        {
          if((xm-x0)==0)
          {
          ang1=90;
          ang2=90-angle;
          }
          else
          {
          ang1=atan((zm-z0)/(xm-x0))*180/PI;
         ang2=atan((zm-z0)/(xm-x0))*180/PI-angle;
          }
        }
        break;
        case 2:
        {
          ang1=180+atan((zm-z0)/(xm-x0))*180/PI;
          ang2=180+atan((zm-z0)/(xm-x0))*180/PI-angle;
          }
          break;
        case 3:
         {
          if((xm-x0)==0)
          {
          ang2=270-angle;
          ang1=270;
          }
          else
          {
           ang1=180+atan((zm-z0)/(xm-x0))*180/PI;
           ang2=180+atan((zm-z0)/(xm-x0))*180/PI-angle;
          }
        }
        break;
        case 4:
        {
          ang2=360+atan((zm-z0)/(xm-x0))*180/PI-angle;
          ang1=360+atan((zm-z0)/(xm-x0))*180/PI;
          }
          break;
        }
      }
        else
         {
    switch(gansmyongde_xz(xm-x0,ym-y0,zm-z0,SN))
      {
        case 1:
        {
         ang2=atan((zm-z0)/(xm-x0))*180/PI+angle;
         ang1=atan((zm-z0)/(xm-x0))*180/PI;
        }
        break;
        case 2:
        {
           if((xm-x0)==0)
          {
          ang2=90+angle;
          ang1=90;
          }
          else
          {
           ang2=180+atan((zm-z0)/(xm-x0))*180/PI+angle;
           ang1=180+atan((zm-z0)/(xm-x0))*180/PI;
          }
          }
          break;
        case 3:
         {
         ang2=180+atan((zm-z0)/(xm-x0))*180/PI+angle;
         ang1=180+atan((zm-z0)/(xm-x0))*180/PI;
        }
        break;
        case 4:
        {
          if((xm-x0)==0)
          {
          ang2=270+angle;
          ang1=270;
          }
          else
          {
           ang2=360-atan((zm-z0)/(xm-x0))*180/PI+angle;
           ang1=360-atan((zm-z0)/(xm-x0))*180/PI;
          }
         }
          break;
        }
      }
        double a1=nijie1(75,25,90,300,xe,ye,ze);
        double a2=nijie2(75,25,90,300,xe,ye,ze);
        double a3=nijie3(75,25,90,300,xe,ye,ze);
         myservo1.write(a1);
         myservo2.write(a2);
         myservo3.write(a3);
         delay(t);//微秒延迟函数delayMicroseconds()
      while(n>0)
      {
       if(SN==0)
       {
         ang1=ang1-ang0;
         xe=r*cos(ang1*PI/180)+x0;
         ze=r*sin(ang1*PI/180)+z0;
         a1=nijie1(75,25,90,300,xe,ye,ze);
         a2=nijie2(75,25,90,300,xe,ye,ze);
         a3=nijie3(75,25,90,300,xe,ye,ze);
         myservo1.write(a1);
         myservo2.write(a2);
         myservo3.write(a3);
         delay(t);//微秒延迟函数delayMicroseconds()
        }
        else
        {
         ang1=ang1+ang0;
         xe=r*cos(ang1*PI/180)+x0;
         ze=r*sin(ang1*PI/180)+z0;
         a1=nijie1(75,25,90,300,xe,ye,ze);
         a2=nijie2(75,25,90,300,xe,ye,ze);
         a3=nijie3(75,25,90,300,xe,ye,ze);
         myservo1.write(a1);
         myservo2.write(a2);
         myservo3.write(a3);
         delay(t);//微秒延迟函数delayMicroseconds()
          }
         n=n-1;
         }
          xe=r*cos(ang2*PI/180)+x0;
          ze=r*sin(ang2*PI/180)+z0;
          a1=nijie1(75,25,90,300,xe,ye,ze);
          a2=nijie2(75,25,90,300,xe,ye,ze);
          a3=nijie3(75,25,90,300,xe,ye,ze);
          myservo1.write(a1);
          myservo2.write(a2);
          myservo3.write(a3);
          delay(t);//微秒延迟函数delayMicroseconds()
        }
    
        
    //xoy平面或与xoy平面平行平面
     //起点象限判断函数,根据插补方向SN和xm,ym的值判断 
    int gansmyongde_xy(double xm,double ym,double 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;
      }
      
       //yoz平面或与yoz平面平行平面
       //起点象限判断函数,根据插补方向SN和xm,ym的值判断 
    int gansmyongde_yz(double xm,double ym,double zm,int SN)
    {
      int i=0;
      if(SN==0)
      {
        if((ym>=0)&&(zm>0))
            i=1;
        if((ym<0)&&(zm>=0))
            i=2;
        if((ym<=0)&&(zm<0))
            i=3;
        if((ym>0)&&(zm<=0))
            i=4;
        }
      else if(SN==1)
      {
        if((ym>0)&&(zm>=0))
            i=1;
        if((ym<=0)&&(zm>0))
            i=2;
        if((ym<0)&&(zm<=0))
            i=3;
        if((ym>=0)&&(zm<0))
            i=4;
        }
        return i;
      }
    
      //xoz平面或与xoz平面平行平面
     //起点象限判断函数,根据插补方向SN和xm,ym的值判断 
    int gansmyongde_xz(double xm,double ym,double zm,int SN)
    {
      int i=0;
      if(SN==0)
      {
        if((xm>=0)&&(zm>0))
            i=1;
        if((xm<0)&&(zm>=0))
            i=2;
        if((xm<=0)&&(zm<0))
            i=3;
        if((xm>0)&&(zm<=0))
            i=4;
        }
      else if(SN==1)
      {
        if((xm>0)&&(zm>=0))
            i=1;
        if((xm<=0)&&(zm>0))
            i=2;
        if((xm<0)&&(zm<=0))
            i=3;
        if((xm>=0)&&(zm<0))
            i=4;
        }
        return i;
      }
    
          提取码:zwvy
    大家的支持是我无限的动力

    1人打赏

  • TA的每日心情
    擦汗
    2019-1-26 16:31
  • 签到天数: 446 天

    [LV.9]以坛为家II

    发表于 2019-1-7 08:26 | 显示全部楼层
    建议部分关键代码可以放到帖子里作为展示用,海报的话我这边感谢下,之前弄好后面忘记提交了。
    PY让世界更美好~
  • TA的每日心情
    慵懒
    2019-9-18 16:39
  • 签到天数: 326 天

    [LV.8]以坛为家I

     楼主| 发表于 2019-1-7 08:59 | 显示全部楼层
    syl312 发表于 2019-1-7 08:26
    建议部分关键代码可以放到帖子里作为展示用,海报的话我这边感谢下,之前弄好后面忘记提交了。 ...

    好的,我编辑一下
    大家的支持是我无限的动力
  • TA的每日心情
    奋斗
    2019-9-17 00:35
  • 签到天数: 48 天

    [LV.5]常住居民I

    发表于 2019-3-14 05:03 | 显示全部楼层
    好东西,不知道楼主能否指点一二?
  • TA的每日心情
    慵懒
    2019-9-18 16:39
  • 签到天数: 326 天

    [LV.8]以坛为家I

     楼主| 发表于 2019-3-14 10:32 | 显示全部楼层
    想学坏的小孩 发表于 2019-3-14 05:03
    好东西,不知道楼主能否指点一二?

    可以的,互相交流
    大家的支持是我无限的动力
  • TA的每日心情
    无聊
    2017-6-24 14:20
  • 签到天数: 3 天

    [LV.2]偶尔看看I

    发表于 2019-3-15 23:43 | 显示全部楼层
    这才是大神啊,算法自己写。太强了。
    打赏

    该用户从未签到

    发表于 2019-3-25 16:28 | 显示全部楼层
    好东西。想与楼主交流。无奈新手不能私信。楼主方便时可否联系我?
  • TA的每日心情
    慵懒
    2019-9-18 16:39
  • 签到天数: 326 天

    [LV.8]以坛为家I

     楼主| 发表于 2019-3-27 09:20 | 显示全部楼层
    taeoltt 发表于 2019-3-25 16:28
    好东西。想与楼主交流。无奈新手不能私信。楼主方便时可否联系我?

    可以加我微zsh1025863915,互相交流
    大家的支持是我无限的动力
  • TA的每日心情
    慵懒
    2019-9-18 16:39
  • 签到天数: 326 天

    [LV.8]以坛为家I

     楼主| 发表于 2019-3-27 09:24 | 显示全部楼层
    190808149 发表于 2019-3-15 23:43
    这才是大神啊,算法自己写。太强了。

    你也可以的
    大家的支持是我无限的动力
  • TA的每日心情
    无聊
    2019-4-25 10:57
  • 签到天数: 2 天

    [LV.1]初来乍到

    发表于 2019-4-3 16:23 | 显示全部楼层
    你好 请问Arduino uno  具有能够实现计算6关节机器人末端的位姿的能力吗?
    您需要登录后才可以回帖 登录 | 立即注册  

    本版积分规则

    热门推荐

    esp32作为智能主机使鱼缸内温度保持在28度种植水草
    esp32作为智能主机使鱼缸
    esp32作为智能主机使鱼缸内温度保持在28度种植水草, 设计原则, 一,使用我手上现有
    请大佬帮助如何分别控制两块pca9685上的舵机
    请大佬帮助如何分别控制两
    目前一块驱动板上的舵机可以分别控制分别是0到15号舵机,不清楚在级联的情况下如何分
    【干货分享】mega2560原理图PCB图纸altium designer18
    【干货分享】mega2560原理
    分享一下mega2560的板子 AD版本 **** 本内容被作者隐藏 **** ergo
    [Arduino物联网开发实战4]天猫精灵控制1
    [Arduino物联网开发实战4]
    [md] 上一篇:(https://www.arduino.cn/thread-83175-1-1.html) blinker支持多种智
    【项目】基于Arduino Nano的多功能桌面感应垃圾桶
    【项目】基于Arduino Nano
    基于Arduino Nano的多功能桌面感应垃圾桶随着大学生活不断往前迈进,宿舍桌面上的杂物
    Copyright   ©2015-2016  Arduino中文社区  Powered by©Discuz!   ( 蜀ICP备14017632号-3 )
    快速回复 返回顶部 返回列表