Code for first robot prototype