Added some error handling and name changes
Added thread
Setup serial signal
Merge branch 'develop' of https://github.com/vtslab/sensafety-midterm into develop