Added servo configurations
Fixed servo pins
Added servo controller
Cleaned up unused code