Changed arduino side code, untested atm...