Updated code from mock 1. PID works, but vision is still finicky.