Basic outline of code ported from Arduino code. Non working currently