V5.1 Fairly Major Update to Sensing+Comms organized

-Separated the get_result function from interim into "getting" and
"outputting" result
-Adjusted the code such that the "cup found" function has a more
complete logical flow by moving the sensing from the "go home" function
-Buildable, not tested