Added pid controller for testing