PD Controller Code