Added vision code for calculating distance

Not Complete