IControlAlforithm added, extracted PID part of SwarmController to PIDAlgorithm