Added Vision! Still needs to be calibrated and tested.
Got it working, tested a bit on robot