Did you think about use one of the many versions of 3D printers firmware (one of those are Marlin)? The home function is similar to what you want. Take a look, and if you can't do anything with that send me a PM and we talk about it (I'm not a professional programmer but I think I cant help).