Cleaned code in robot_main and imu