همه موارد درست به نظر می آید.
فقط یک نکته را احتمالا فراموش کرده اید، چون در برنامه قبلی بود و آن هم شرط صفر بودن W1 هنگام خواندن از سریال پورت است.
در ضمن زمانی که با شرط یک بودن W1 به تابع WSlave رفته اید دیگر نیازی نیست درون خود تابع هم W1 را چک کنید.
با اصلاح این دو نکته برنامه را برایتان آپلود کردم.