Fixed bugs causing program to get stuck.

Fixed issues relating to clock frequency changes and millis() rollover.