Added servocontrol class

Init of PCA9685 and servo init is now done from servocontrol class