C code is almost done.

Just need to hook it up to the 555 timer to check if it works.