line_inter_3d.m

使用Matlab的M文件编写的数控三轴直线运动代码

function line_inter_3d(xe, ye, ze, fc, fs, fe, at)
%3d line interpolate
%input x end,y end,z end,feed velocity,speed rising time
%draw the interpolate points of this line
%INT16 xe, int16 ye;
%unit16 fc, uint16 fs, uint16 fe;
%uint16 a_t;

%变量声明
int16 xe;
int16 ye;
int16 ze;
int16 fc;
int16 fs;
int16 fe;
int16 at;
int16 fi;
fi = int16(0);
double xi;
double yi;
double zi;
double dx;
double dy;
double dz;
char x_dir;
char y_dir;
char z_dir;
double L;
double La;
double Le;
double Lsum;
double Lxy;
double dL;
double dLxy;     %XY平面周期进给量
double cos_z;    %L与XY平面夹角余弦值
double sin_z;    %L与XY平面夹角正弦值
double tan_xy;   %Y与X夹角正切值
double sec_xy;   %Y与X夹角余弦值
int16 cParam;

%变量初始化
xi = double(0);
yi = double(0);
zi = double(0);
acc_flag = char(0);
ac_step = char(0);
end_step = char(0);
x_dir = char(0);
y_dir = char(0);
z_dir = char(0);
blnRun = char(1);
dx = double(0);
dy = double(0);
dz = double(0);
dL = double(0);
cos_z = double(0);
sin_z = double(0);
tan_xy = double(0);
sec_xy = double(0);
L = double(0);
La = double(0);
Le = double(0);
Lsum = double(0);
Lxy = double(0);
temp = int32(0);
acc_step = int16(0);
cParam = int16(125);

%设置绘图模式为“添加”
hold on
grid on
box on

%判定x进给方向
if (xe < 0)
    xe = -xe;
    x_dir = 1;
end

%判定y进给方向
if (ye < 0)
    ye = -ye;
    y_dir = 1;
end

%判定y进给方向
if (ze < 0)
    ze = -ze;
    z_dir = 1;
end

%若本段无进给
if(xe==0 && ye==0 && Ze==0)
    blnRun = 0;
    return;
end

%求Lxy长度
Lxy = sqrt(double(xe*xe + ye*ye));
L   = sqrt(double(xe*xe + ye*ye + ze*ze));
if(ze ~= 0)
    %求L与XY平面夹角cos值
    cos_z = Lxy/L;

    %求L与XY平面夹角sin值
    sin_z = sqrt(double(1 - cos_z*cos_z));

    if(xe ~= 0)
        %计算tan值
        tan_xy  = ye/xe;
        %计算sec值
        sec_xy  = sqrt(double(tan_xy*tan_xy + 1));
    end
end

%速度预处理段,采用梯形加减速控制
if(fs < fc)
    %加速过程
    acc_flag = 0;
    ac = int16((fc - fs)/at);
elseif (fs == fc)
    %匀速过程
    acc_flag = 1;
    if(fe < fc)
        ac = int16((fc - fe)/at);
    end
elseif(fs > fc)
    %减速过程
    acc_flag = 2;
    ac = int16((fs - fc)/at);
end

%为了与DSP上插补时间一致，此处乘以时间系数
ac = ac*125;

while (blnRun)
    switch(acc_flag)
        case 0
            %加速段
            %梯形加速控制，加速度值不变
            %速度呈线性变化
            fi = fi + ac;
            Le = L - Lsum;

            if(fi >= fc)
                %升速阶段完成，置恒速运行
                acc_flag = 1;
                La = Lsum + dL;
                fi = fc;
            end

            if(Le <= La)
                %越过加速区，进入减速区
                acc_flag = 2;
            end

            dL = fi/cParam;
            acc_step = acc_step + 1;
        case 1
            %匀速段
            Le = L - Lsum;

            if(Le <= La)
                acc_flag = 2;
            end
        case 2
            %减速段
            fi = fi - ac;
            dL = fi/cParam;
            acc_step = acc_step - 1;

            if(ac_step <= 1)
                acc_flag = 3;
                %if(dL == 0)
                 %   dL = 1;
                %end
                end_step = int16((La - Le)/dL);
                if(end_step <= 0)
                    blnRun = 0;
                end
            end
        case 3
            %低速段
            end_step = end_step - 1;
            if(end_step <= 0)
                blnRun = 0;
            end
    end
    
    %求qdLxy
    dLxy = dL*cos_z;
    
    if(ze ~= 0)
        dz = dL*sin_z;
    else
        dz = 0;
    end
    if (xe ~= 0)
        dx = int16(double(dLxy)/sec_xy);
        dy = int16(double(dx)*tan_xy);
    else
        dx = 0;
        dy = dLxy;
    end

    %重新计算dL值，减少舍入误差
    dL = sqrt(double(dx*dx + dy*dy + dz*dz));

    xi = xi + dx;
    yi = yi + dy;
    zi = zi + dz;

    Lsum = Lsum + dL;
    if(Lsum >= L)
        dL = L - (Lsum - dL);
        blnRun  = 0;
    end
    
    if(x_dir == 1)
        dx = -dx;
    end
    if(y_dir == 1)
        dy = -dy;
    end
    if(z_dir == 1)
        dz = -dz;
    end

    double x;
    double y;
    double z;
    x = double(xi);
    y = double(yi);
    z = double(zi);
    for i = 1:1:dx
        x = x + 1;
        y = y + tan_xy;
        z = z + (sec_xy*sin_z/cos_z);
        %line(x, y);
        plot3(x, y, z, 'r*');
    end
end
xlabel('X-axis');
ylabel('Y-axis');
zlabel('Z-axis');
axis([0, xe, 0, ye, 0, ze]);