Fixed G31 for Z endstop switch

When using a Z endstop switch, one could not specify a switch offset by using G31 Z. This commit fixes this issue - tested and working.