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