涂鸦arduino中文社区活动
查看: 22737|回复: 81

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

  [复制链接]

该用户从未签到

发表于 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
再将运动学写成代码即可,如下所示为逆解代码:
[mw_shl_code=arduino,true]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);//返回角度
}[/mw_shl_code]

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


圆弧插补:圆弧插补和直线插补类似,对于任意一段平面圆弧上,我们需要找到圆弧起点到终点过程的插补点。如图所示为圆弧插补图,我们选任意一圆心点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

          程序源码我上传了百度云,需要的可以自行下载:
[mw_shl_code=arduino,true]#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;
  }
[/mw_shl_code]
      提取码:zwvy

该用户从未签到

 楼主| 发表于 2020-12-27 13:17 | 显示全部楼层
本帖最后由 AimHigh 于 2021-2-7 18:02 编辑

感兴趣的朋友可以去网易云课堂搜索:
1. 机械手运动学:https://study.163.com/course/introduction/1208982813.htm
2. 机械手轨迹控制:https://study.163.com/course/introduction/1211056801.htm
最近有人私信我想移植到工业,在此强调一下,我在论坛这里开源的是arduino+舵机的玩具,算法都是直角坐标系(数控)的初级算法,毫无精度可言,千万别想着移植到工业,那样纯粹浪费时间,千万别走弯路
对于工业我做的都是stm32+步进电机或者伺服电机的,算法跟玩具的完全不同,有速度规划,轨迹规划,基于时间分割的空间插补嵌套S速度控制等等,工业方案我是不开源的,是有偿的,建议有一点经济能力的人或者想省事的人来购买,其他人想搞工业的,不花钱想自己研究的,建议去SCI和EI多看看文章,将算法看懂了写成了程序即可

重要的事情再说一下哈,这里开源的是玩具,千万别移植到工业,不然花了时间还达不到你想要的效果,甚至造成一些损失的千万别私信我来抱怨我哈

该用户从未签到

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

该用户从未签到

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

好的,我编辑一下

该用户从未签到

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

该用户从未签到

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

可以的,互相交流

该用户从未签到

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

该用户从未签到

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

该用户从未签到

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

可以加我微zsh1025863915,互相交流

该用户从未签到

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

你也可以的

该用户从未签到

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

本版积分规则

热门推荐

arduinoT12焊台 控制器 炫酷UI
arduinoT12焊台 控制器 炫
*声明:本项目源于https://github.com/wagiminator/ATmega-Soldering-Station 采用CC3
arduino uno 读取不了DHT11数据,求助大佬!!!
arduino uno 读取不了DHT1
整了两天了,还是没整出来,换了线,换了传感器都不行!就差换板子了。求助各位大佬。
Arduino引脚图
Arduino引脚图
Arduino UNO R3 (CH340G)引脚 持续更新中·····
ESP8266/RJ45网口CH9121/POE供电/USB转TTL口网络继电器开发板
ESP8266/RJ45网口CH9121/P
四路继电器控制板带RJ45网口CH9121,POE供电,USB转TTL口,Type-C接口,STC单片机冷启E
Arduino物联网开发实例教程
Arduino物联网开发实例教
[md] **blinker用户交流群 301438087 (仅限github已Star的用户加群)** ## bli
Copyright   ©2015-2016  Arduino中文社区  Powered by©Discuz!   
快速回复 返回顶部 返回列表