Added first pass of visual servo logic