Vision working with motor feedback! Also cleaned up a bunch of stuff.