Fixed Timing

Timing millis() and micros() now work, seem to be correct. Fixed issue
with debug_printf() occasionally breaking program.

Worked on benchmarking methods and tested code.