trapezoid motion profile now working somewhat, still a bit of calculation error