五轴摇篮AC/BC刀尖跟随RTCP算法

Tualar

//支持3+2定轴G54变化的计算
//支持五轴联动刀尖点变化的跟踪

#define rad     ((double)0.01745329252)  /*角度转换为弧度π/180*/
#define AC       (UINT)0  //AC摇篮机床
#define BC       (UINT)1  //BC摇篮机床

typedef struct
{
        double x, y, z, axle4, axle5; //基于工件坐标系的NC指令数据G1 X,Y,Z,A/B,C。axle4=A/B轴角度,axle5=C轴角度。
        double x5, y5, z5, x4, y4, z4; //用来接收旋转轴转动后刀尖点位置变化的计算结果, 最终RTCP计算结果保存在x4,y4,z4里面。
        double rcpx, rcpy, rcpz; //第四轴A/B摇篮中心线和第五轴转台中心线的交点在机床坐标系中的位置。通过标定找到回转中心。
        double coordx, coordy, coordz; //分中/工件坐标系G54数据。
}rtcp_t;

//RTCP计算函数：
//参数p:        指向rtcp_t结构的指针。
//参数type:   机床结构力学，取AC或者BC。
//返回值:      TRUE表示计算成功; FALSE表示计算失败。
BOOL RTCP_Algorithm(rtcp_t* p, UINT type)
{
        if ( !p || ( type != AC && type != BC)) return FALSE;
       
        //先计算五轴转台:
        double x, y, z, X, Y, Z;
        double sin4, cos4, sin5, cos5;

        sin5 = sin(p->axle5*rad);
        cos5 = cos(p->axle5*rad);

        x = p->rcpx + p->coordx + p->x;
        y = p->rcpy + p->coordy + p->y;
        z = p->rcpz + p->coordz + p->z;

        X = x*sin5 + y*cos5;
        Y = x*cos5 - y*sin5;
        Z = z;

        p->x5 = X - p->rcpx - p->coordx;
        p->y5 = Y - p->rcpy - p->coordy;
        p->z5 = Z - p->rcpz - p->coordz;

       //后计算四轴摇篮:
        x = p->rcpx + p->coordx + p->x5;
        y = p->rcpy + p->coordy + p->y5;
        z = p->rcpz + p->coordz + p->z5;

        sin4 = sin(-(p->axle4)*rad);
        cos4 = cos(-(p->axle4)*rad);

        switch (type)
        {
                case AC:
                        X = x;
                        Y = y*cos4 - z*sin4;
                        Z = y*sin4 + z*cos4;
                        break;
                case BC:
                        X = x*cos4 + z*sin4;
                        Y = y;
                        Z = -x*sin4 + z*cos4;
                        break;
        }

        //计算结果保存在X4,Y4,Z4变量中：
        p->x4 = X - p->rcpx - p->coordx;  
        p->y4 = Y - p->rcpy - p->coordy;
        p->z4 = Z - p->rcpz - p->coordz;

       return TRUE;
}