Refactor, attempting to move away from arduino library