Add unit tests, refactor timers, pins