fixed some servo bugs