More commands and SGR bugfixed
Fixed
Removed debug message that crashes arduino initialization
Some minor fixes
Added message print methods and fixed input read.
Removed unused debug function.
Moved 'ansi.h' include