Working on servo write/read

Initial test implementation working but functions not finished yet.