Initial commit, removed refocusing and added C++ rPi code