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

            IF t > t_acc THEN
                csp_progress :=uni;
            END_IF

        uni:
            tap :=LREAL_TO_DINT(acp_init + 0.5 * a_acc*EXPT(t_acc,2)+ v_uni*(t-t_acc) );
            t := t + cyc_time;  
            IF t > t_tot-t_acc THEN     
                csp_progress :=dec;
            END_IF

        dec:
            tap :=LREAL_TO_DINT(acp_init+x_tot-0.5* a_acc*EXPT(t_tot-t,2)) ;
            t := t + cyc_time;
            IF t >  t_tot THEN
                csp_progress := fin;
            END_IF

        fin:
            power_do:= FALSE;
            t:=0;   
            acp_init:=acp;  
            IF csp_do THEN
            csp_progress :=pre;
            END_IF  

    END_CASE
END_IF

3.按一下走一段相对位移。