Marlin中delta结构的笛卡尔空间坐标和电机轴坐标的转换详解.doc

Marlin固件中需要将从Gcode中获取的待打印器件的3D模型中的坐标，转换为三角洲3D打印机的三个电机轴上的坐标。 电机轴指的是三个电机对应的线轨。 电机轴坐标指的是滑块在电机轴上的位置。 笛卡尔空间坐标是指从Gcode中获取的待打印器件的3D模型中的坐标。 实现坐标转换功能的函数为calculate_delta()。 变量中的tower指的是电机轴，tower1，tower2和tower3的位置如下图所示 变量delta_tower1_x为电机轴tower1在笛卡尔坐标系中坐标x的值， 变量delta_tower1_y为电机轴tower1在笛卡尔坐标系中坐标y的值， 变量delta_tower2_x为电机轴tower2在笛卡尔坐标系中坐标x的值， 变量delta_tower2_y，delta_tower3_x，delta_tower3_y类似。 这三个电机轴转换到笛卡尔坐标系后的坐标如代码所示，原理如下图 这是三角洲3D打印机抽象的俯视图。点tower1，tower2和tower3分别为三个电机轴（因为是俯视图，所以在图中表示为一个点），这三个点构成一个等边三角形，内切圆的圆心为点z，内切圆的半径为delta_radius。 笛卡尔坐标系的原点为等边三角形内切圆的圆心，即点z；x轴穿过原点，并且与tower1和toweer2组成的线段平行；y轴经过tower3和原点；z轴经过原点并且与tower3平行。 电机轴tower1转换到笛卡尔坐标系的坐标x为点M，坐标y为点N。 电机轴tower2转换到笛卡尔坐标系的坐标x为点P，坐标y为点N。 电机轴tower3转换到笛卡尔坐标系的坐标x为0，坐标y为tower3本身。 M的坐标为(-sin60 * delta_radius, 0), N的坐标为(0, -cos60 * delta_radius), P的坐标为(sin60 * delta_radius, 0) 所以，tower1在笛卡尔空间坐标系中的x坐标delta_tower1_x= -sin60 * delta_radius,依次类推 delta_tower1_y = -cos60 * delta_radius , delta_tower2_x = sin60 * delta_radius , delta_tower2_y = -cos60 * delta_radius , delta_tower3_x = 0 , delta_tower3_y = delta_radius , 再来看下代码是不是这样的。 回过头来看看是怎么计算电机轴上滑块位置的。假设有下图 点A为喷头的位置， 点B为点A投影到电机轴tower2（也是平行于笛卡尔坐标系的Z轴）， 点C为滑块的位置， 点D为点A投影到笛卡尔坐标系的xy平面的点， 点E为电机轴tower2与笛卡尔坐标系的xy平面相交的点。 其中BE就是打印物品笛卡尔坐标系的z值，AC为推杆长度，AB垂直于BC，根据直角三角形斜边平方等于两直角边平方之和，即 AC*AC = AB *AB + BC*BC 其中AC已知，为推杆长度，AB可以通过喷头在笛卡尔坐标系的X坐标和Y坐标算出来，所以BC也就可以算出来了。 AB = DE XY平面如下所示 点D为前一个图点A的投影，点E为点B的投影，点A为喷头的坐标，那么点D的xy坐标也是喷头的xy坐标，是已知的。点E电机轴tower2在XY平面的点，坐标也是已知的，点K和I分别为点E在X和Y轴的投影。 那么DH = OK - OG ，即E的x坐标 - 喷头的x坐标 EH = FI = OI - OF ， 即E的y坐标 - 喷头的y坐标 DE*DE = EH*EH + DH*DH 综上所述，滑块的z轴坐标 = BE + BC =喷头的z坐标 + BC =喷头的z坐标 + sqrt(推杆长度*推杆长度 - AB*AB) =喷头的z坐标 + sqrt(推杆长度*推杆长度 - (EH*EH + DH*DH)) =喷头的z坐标 + sqrt(推杆长度*推杆长度 - ((tower2的y坐标 - 喷头的y坐标) * (tower2的y坐标 - 喷头的y坐标) + (tower2的x坐标 - 喷头的x坐标) * (tower2的x坐标 - 喷头的x坐标))) 对应代码为 delta[TOWER_2] = sqrt(delta_diagonal_rod_2_tower_2 - sq(delta_tower2_x - cartesian[X_AXIS]) - sq(delta_tower2_y - cartesian[Y_AXIS