That code is just what I was thinking. Nice and simple, very little changed in the original toolchange program.
Nice to see your making progress. Post video of it when your done. Im jealous already. Want this on my machine but I really depend on her these days and cant really afford the downtime, and risk of creating more problems.