add-pid-algorithm-first-stage

need further debug after import pic ctrl