Fixed arduino code, now working with any length