Updated to robot code

Finalliy pushing robot code