Revert "removed pointless wait in main_teleop.c"

This reverts commit 97b9e7be885c8ed7b571c2c35f5b3a13627e8601.