Move arduino code
Implement somewhat working implementation