Ported OpenTopServo to cpp