Added vision code for calculating distance

Not complete