TwinCAT写了个CSV模式的梯形加减速

1.学习中需要对6040进行pdo通信，所以内置的模块就没法用了，就写了些简单的程序。
2.程序的思想就是实时给60FF速度值，这里用的是17位的编码器，速度值需乘131072，这里加减速给的是相同的。

IF in_csv THEN
v_acc:=v_give*131072;
CASE csv_progress OF        
        csv_pre:
            power_do:= TRUE;
            t:=0;
            IF ton1_q THEN
            csv_progress :=csv_acc;
            END_IF

        csv_acc:
            tav :=LREAL_TO_DINT(0+v_acc*t) ;
            t := t + cyc_time;

            IF t > 1 THEN
                csv_progress :=csv_uni;
            END_IF

        csv_uni:
            tav :=LREAL_TO_DINT(v_acc);         
            IF stop_do THEN
                csv_progress :=csv_dec;
            END_IF

        csv_dec:
            tav :=LREAL_TO_DINT(v_acc-v_acc*(1-t)) ;
            t := t - cyc_time;
            IF t < 0 THEN
                csv_progress := csv_fin;
            END_IF

        csv_fin:
            power_do:= FALSE;
            t:=0;       
            tav :=0;
            IF csv_do THEN
                csv_progress :=pre;
            END_IF          
    END_CASE
END_IF

3.结果我就不运行了，速度模式比较简单。