add blocking thread for cone
correct automata and servo driving