line_inter_2d.m

Matlab的M文件编写的数控插补2D直线算法仿真。具体参数可使用：X-500,Y-600

function line_interpolate_2d(xe, ye, fc, fs, fe, at)
%2d line interpolate
%input x end ,y 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 fc;
int16 fs;
int16 fe;
int16 at;
int16 fi;
fi = int16(0);
double xi;
double yi;
double L;
double La;
double Le;
double Lsum;
double dL;
int dx;
int dy;
double tan_xy;
double sec_xy;
int16 cParam;

xi = double(0);
yi = double(0);
acc_flag = char(0);
ac_step = char(0);
end_step = char(0);
x_dir = char(0);
y_dir = char(0);
blnRun = char(1);
dx = double(0);
dy = double(0);
dL = double(0);
tan_xy = double(0);
sec_xy = double(0);
L = double(0);
La = double(0);
Le = double(0);
Lsum = 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

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

if(blnRun)
    L = sqrt(double(xe*xe + ye*ye));

    if(xe ~= 0)
        %计算tan值
        tan_xy = ye/xe;
        %计算sec值
        %temp = tan_xy*tan_xy;
        sec_xy = sqrt(tan_xy*tan_xy + 1);
    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;
end

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;
                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

    if (xe ~= 0)
        dx = double(dL)/sec_xy;
        dy = double(dx)*tan_xy;    
    else
        dx = 0;
        dy = dL;
    end
    
    %重新计算dL值，减少舍入误差
    dL = sqrt(double(dx*dx + dy*dy));
    
    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

    double x;
    double y;
    x = double(xi);
    y = double(yi);
    for i=1:1:dx
        x = x + 1;
        y = y + tan_xy; 
        %line(x, y);
        plot(x, y, 'r*');
    end
    
    xi = xi + dx;
    yi = yi + dy;
    
end
i=1;
xlabel('X-axis');
ylabel('Y-axis');
axis([0, xe, 0, ye]);