基于arduino uno的delta并联机器人控制系统设计教程三
本帖最后由 AimHigh 于 2018-9-15 11:06 编辑教程三来教大家如何写出delta并联机器人中圆弧插补的代码
圆弧插补和直线插补类似，对于任意一段平面圆弧上，我们需要找到圆弧起点到终点过程的插补点。如下图所示为圆弧插补图，我们选任意一圆心点O’，其坐标是O’(x0,y0,z0),A点为初始点，其坐标为（xm,ym,zm）,若以O为圆心，OA为半径，逆时针转b个角度，到达B点，实现以A到B逆时针圆弧插补。：
令邻近两插补点距离为h，且点都在圆上，如图AC距离为h。c为AC段圆弧对应的圆心角，因为h是定值，所以c为固定值。即：
其中r为圆弧半径为：
由上面分析得到平面圆弧插补流程图如下图所示，其中（xm,ym,zm）为起点坐标，(x0,y0,z0)为圆心坐标，angle为插补角度，h为插补精度，SN为插补方向（SN=0为顺时针，SN=1为逆时针），t为插补一次的时间间隔。 然后我们就可以在arduino IDE中写入相应的代码：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;
}
页:
[1]