Added Z depth algor. for all holes, still need modbus