readServo test function now working

implementation is not scalable at the moment and needs reviewing.