Решил задачу, вот тут полный файлик управляющий моим роботом
http://forum.hiasm.com/forum_serv.php?q=56&id=3750