Self Balancing Robot Coding Issue