Debugged all the copy-pasted code from last year, added in sonar sensor just to fix errors, not on robot.