Orientation code mostly working, will probably write a better function two sensors for accuracy, and will get corners working