added robot-c segway debugged working code