Fixed some bugs and added a new command

Fixed some bugs with serial connection and added a new command