Inital commit, tested works on Arduino UNO