基于arduino的delta并联机械手-Arduino中文社区 - Powered by Discuz!
设为首页收藏本站

Arduino中文社区

 找回密码
 立即注册

QQ登录

只需一步，快速开始

Arduino中文社区»首页 › 大赛专区 › 开源硬件开发大赛 › 基于arduino的delta并联机械手
12345678910... 11下一页
返回列表 发新帖
查看: 80478|回复: 109

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

  [复制链接]
AimHigh
发表于 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
回复

举报

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

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

举报

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

举报

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

好的，我编辑一下
回复 支持 反对

举报

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

举报

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

可以的，互相交流
回复 支持 反对

举报

190808149
发表于 2019-3-15 23:43 | 显示全部楼层
这才是大神啊，算法自己写。太强了。
回复 支持 反对

举报

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

举报

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

可以加我微zsh1025863915，互相交流
回复 支持 反对

举报

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

你也可以的
回复 支持 反对

举报

zhouweijun
发表于 2019-4-3 16:23 | 显示全部楼层
你好 请问Arduino uno  具有能够实现计算6关节机器人末端的位姿的能力吗？
回复 支持 反对

举报

下一页 »
12345678910... 11下一页
返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

小黑屋|Archiver|手机版|Arduino中文社区

GMT+8, 2024-11-27 23:54 , Processed in 0.112091 second(s), 20 queries .

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

快速回复 返回顶部 返回列表