new pid controller

based on proper units (dps) not flight tested