Added self control and basic PID