updated working version with servo and sonar support