Initial commit with already started servocontrol library