Working integrated code (as far as I know)

Tracking angle set hasn't been tested. Tracking mode works.