基于arduino的delta并联机械手
delta并联机械手现在广泛应用于工业，因为资金有限，用舵机做的，底座这些也是用的实验室抽屉打孔固定，已完工。首先说一下我的整体方案设计见图，它包括三个模块分别是舵机执行模块、电磁铁执行模块和串口通讯模块。其中舵机模块也是最重要的模块，需要三个舵机实现插补完成在空间走任意线段，定点搬运打孔等功能。
**** Hidden Message *****
建立如图所示的动、静两个坐标系，静坐标系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个位置逆解如下： 再将运动学写成代码即可，如下所示为逆解代码：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)。如图为直线插补图很简单的就可以求出BC直线段的距离，为了简化表述，做了流程图，插补具体流程图如下所示： 根据流程图将空间直线插补代码写入即可，如下所示：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逆时针圆弧插补。 其中（xm,ym,zm）为起点坐标，(x0,y0,z0)为圆心坐标，angle为插补角度，h为插补精度，SN为插补方向（SN=0为顺时针，SN=1为逆时针），t为插补一次的时间间隔。 同样的，跟直线插补一样，根据流程图将空间圆弧插补写好即可。 通过上述功能就可以实现走空间任意线段，其运动速度可以慢/中/快（改变延时即可），拍了个简单的视频如下（直线比较简单，就没拍了）https://v.youku.com/v_show/id_XNDExNDU2MjgwOA==.html?spm=a2hzp.8244740.0.0 还需要实现搬运或打孔，只需要末端执行器上装个吸盘或者电机孔就行，我在实验室找了一个小的吸盘装上，控制原理很简单 因为吸盘是12V的，所以要用继电器，到时继电器与吸盘之间记得外接12V的电源，然后常开的就行，关于代码怎么写，具体看你继电器的使用了，我的是送高电平就行，拍了简单的搬运视频如下http://v.youku.com/v_show/id_XMzk5OTU2ODEzNg==.html?spm=a2h3j.8428770.3416059.1。 关于总的电路怎么接，很简单，就是arduino与3舵机相连，舵机接法也比较简单，如下图，照着接就行，继电器接法就看上面说的就好。
关于机械结构的话，底座和靠背都是用的抽屉做的，起固定作用，主动臂与被动臂某宝买的，报尺寸就行。静平台的话就需要自己设计一下，要考虑主动臂在旋转过程中与其干涉的问题，还要特别注意三个电机的安装位置，因为只有保证电机的安装位置在正三角形的三个顶点上，机器人运动时精度才能保证。同时还要注意静平台以机架之间的安装孔的设计。 动平台就需要配合臂，实物图如下
程序源码我上传了百度云，需要的可以自行下载：#include
Servo myservo1,myservo2,myservo3;
String comdata = "";
int relayPin =3;
int numdata = {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=numdata*10 - (comdata - '0');
// }
// else
// {
// numdata=numdata * 10 + (comdata - '0');
// }
// }
// }
// }
// comdata = String("");
// if(numdata==0)
// {
// int xe=numdata;
// int ye=numdata;
// int ze=numdata;
// int h=numdata;
// int t=numdata;
// chabu_line(xm,ym,zm,xe,ye,ze,h,t);
// delay(2);
// xm=xe;
// ym=ye;
// zm=ze;
// }
// else if(numdata==1)
// {
// double x0=numdata;
// double y0=numdata;
// double z0=numdata;
// double angle=numdata;
// double h=numdata;
// int SN=numdata;
// int t=numdata;
// 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;
}
链接：https://pan.baidu.com/s/1jEcnOZPLjHcDF0e8PkOPCA 提取码：zwvy 感兴趣的朋友可以去网易云课堂搜索：
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/courseMain.htm?share=2&shareId=480000001859462&courseId=1212188807&_trace_c_p_k2_=a0d3eb907931468d9790d43779350424
最近有人私信我想移植到工业，在此强调一下，我在论坛这里开源的是arduino+舵机的玩具，算法都是直角坐标系（数控）的初级算法，毫无精度可言，千万别想着移植到工业，那样纯粹浪费时间，千万别走弯路
对于工业我做的都是stm32+步进电机或者伺服电机的，算法跟玩具的完全不同，有速度规划，轨迹规划，基于时间分割的空间插补嵌套S速度控制等等，工业方案我是不开源的，是有偿的，建议有一点经济能力的人或者想省事的人来购买，其他人想搞工业的，不花钱想自己研究的，建议去SCI和EI多看看文章，将算法看懂了写成了程序即可
重要的事情再说一下哈，这里开源的是玩具，千万别移植到工业，不然花了时间还达不到你想要的效果，甚至造成一些损失的千万别私信我来抱怨我哈 建议部分关键代码可以放到帖子里作为展示用，海报的话我这边感谢下，之前弄好后面忘记提交了。 syl312 发表于 2019-1-7 08:26
建议部分关键代码可以放到帖子里作为展示用，海报的话我这边感谢下，之前弄好后面忘记提交了。 ...
好的，我编辑一下 好东西,不知道楼主能否指点一二? 想学坏的小孩 发表于 2019-3-14 05:03
好东西,不知道楼主能否指点一二?
可以的，互相交流 这才是大神啊，算法自己写。太强了。 好东西。想与楼主交流。无奈新手不能私信。楼主方便时可否联系我？ taeoltt 发表于 2019-3-25 16:28
好东西。想与楼主交流。无奈新手不能私信。楼主方便时可否联系我？
可以加我微zsh1025863915，互相交流 190808149 发表于 2019-3-15 23:43
这才是大神啊，算法自己写。太强了。
你也可以的 你好 请问Arduino uno具有能够实现计算6关节机器人末端的位姿的能力吗？