Closed Loop Issue

Cpk_Rbt wrote: I'm going to move everything to the test-thread and rearrange them to try to get some meaningful results.!

It might be better to change the period of the servo thread and remove the test-thread.

Removed the test thread, left servo thread its default time.
Changed EtherCAT .xml configuration to treat position data as floats and scale on the fly.

Old:
<pdoEntry idx="60C1" subIdx="01" bitLen="32" halPin="poscommand" halType="s32"/>
New:
<pdoEntry idx="60C1" subIdx="01" bitLen="32" halPin="poscommand" halType="float" scale="18204.604"/>

I had them set as s32 types because I thought they had to match how the drive handles them. Changed them to floats with no problems, which opened up the door to using the built-in scaling and eliminated all the conv_float_s32 and mult2 functions I was using to convert the data types and scale the values.