Fixed G32 auto bed compensation

Fixed bug introduced at version 057o-dc42 that broke the G32 bed
compensation. Added the Z heights of the bed compensation points to the
information displayed by M111 S2. Simplified the code that does
Z-probing in fast and slow stages. Added function sncatf and used it to
simplify the code in several places.