Added integral terms to elevator PID