add p+i for velocity limit