Have you tried that program in the A20? I think it will work.
Generally, #5041, #5042, and #5043 (current X,Z,Y locations) are universal across machines. I am not specifically familiar with the A20, but even between mitsubishi and fanuc, the #5041,2,3 variables are the same. My guess is that you could use that exact program. It looks right to me at least.
So throw it in an A20 and see if it works. My guess is that it works as is.
Let us know if it works. If it fails, tell me where it hangs up or alarms and I ill help you troubleshoot it.