Finished implementation of Scara-Functions

from QHarley
Homing is working
maths tested forward and reverse
but still bug by using M360 or G1
It does some strange movements. Look like Motors are controlled strange