Motion controller is working kindve, maths is not