Some stupid errors fixed
Correcting the 2pi problem for the home position