I'll give it a try. I'm just using a cheap kw12 switch and a hitec HS65HB servo. This is my first printer (prusa i3 rework), so i'm still dialing it in and getting a feel for it. The bed is con-vexed in the center once it warms up. I was getting pretty inconsistent prints. Once I enabled auto bed leveling and used 25 probe points, everything magically started printing good.

I like your code, it'll at least give me a baseline to work off of to see if the switch might be failing.