Fixed Z axis issues temporarily, now handles stairs correctly!