Added rotation velocity saturation, still missing a D term