基于arduino uno的delta并联机器人控制系统设计教程二
本帖最后由 AimHigh 于 2018-9-15 11:07 编辑教程二来教大家如何写出delta并联机器人中直线插补的代码。
插补即是完成轮廓起点到终点之间的中间点坐标计算，并进行脉冲分配。因为舵机的控制信号是脉宽调制（PWM）信号，从A点到另外另A点，在角度允许的范围内，对每个舵机发一次PWM信号即可到达指定点。但因为舵机的反应速度不大且三个舵机一般转动的角度不一致，导致三个舵机不是同时到达指定角度。那么从A点到B点不会是一条直线，会有波动。但当A点到B点距离很短时，这时候三个舵机转动的角度都很小则三个舵机到达各自指定角度的时间间隔会很小，精度也就会提高。
假设机器人动平台在空间坐标系中通过直线插补从A点运动到B点，插补精度为h，插补点时间间隔t。A点坐标为（xm,ym,zm）,B点坐标为（xe,ye,ze）。C点为插补过程点(x1,y1,z1)。建立直线插补图如下：
则可以求出BC直线段的距离：若从C点向B点移动h,则C点x、y和z轴的变化量将分别为：有了上面分析，则可以得到直线插补流程图如图所示：然后我们就可以在arduino IDE中写入相应的代码：//直线插补函数/*此函数是实现在空间直线插补功能 */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; }}}
页:
[1]