Update PID Controller

add agg pid

Signed-off-by: Nattadej Waiyatharee <the.phantom.nat@gmail.com>