Motion profile generator.

[richard]

this works on the real servo motor. what a difference the qei module makes ,and 14 bit pwm seems much smoother
pid rate now 1ms ramp rate 8ms with 49 steps , no attempt yet to handle short moves but long ones are good . fwd and backwards moves are good .
pos counter 24bit / vmax and accel are fixed constants so far