Implemented servo functionality for drive system(Motor Lab)