Added some servo control
Fixed a bounding error in bluetooth reading