Added PID control algorithm, first revision.