Added serial Speed debug
Fixed bugs
*This version was tested on Hardware (Arduino Uno)