not compiled changes for nicer pointers, more comments, better structure and combines velocity and position control