Added function

-Added all sensor functions besides the check wall ones
-Added some movement functions
-Have not tested anything yet