MODBUS Write implemented properly with basic testing