Не думаю, что ПИД поможет. ПИД - это, все-таки, больше про поддержание заданного положения. А у вас задача на перемещение.