Actual Arduino code used, finally working