PI-D Lag

Hi

I'm trying to program a PI-D Lag control in a AKD PDMM in Structure Text, this to control torque of a C062C motor to follow a profile  to achieve linear force with a maximum of 2700N, however, i have several issues with speed response. i program my algorithm in difference equations and i use arrays from 0 to 10,000, then restart the counting to not overflow the arrays. I don't know why but if i don't used a wait_time the system get crazy, so i use a 2ms wait, but it still have an aggressive behavior, with 50ms it works fine. but is a requirement of this application to change torque in relation of a cam profile of 100 points in 1 second each. So it needs to react in less than 10ms. First i did it in matlab and ir works fine, so it take me to the question: Does the AKD PDMM have the capacity to achieve this behavior?. to read linear force i used a load cell, witch i read on the analog input of the AKD.

Thanks