Added actual robot code

Implemented and cleaned up Java Robot code sent to me by Evan