Fix serial parsing on mega and add testing code for uno