Updated after testing on Arduino