Started implementing pid control