Code for a self balancing robot