Add M400 which waits for all moves to complete before returning ok