TwinCAT写了个CSP模式的相对位移（三次多项式插值）

1.思想就是607A不断给值，其中要注意的是NC轴刷新周期，PLC轴刷新周期（默认与NC一致），不然会达不到想要的速度。
2.这个写的是个相对位移

IF in_csp THEN
a_acc:=v_uni/t_acc;
t_tot:=2*t_acc+(x_tot- a_acc*EXPT(t_acc,2))/v_uni;
CASE csp_progress OF        
        pre:
            power_do:= TRUE;
            t:=0;
            acp_init:=acp;
            IF ton1_q THEN
            csp_progress :=acc;
            END_IF

        acc:
            tap :=LREAL_TO_DINT(acp_init + 0.5 * a_acc*EXPT(t,2)) ;
            t := t + cy