Советую автору поумерить свой пыл. С вероятностью -> 1, текущая постановка задачи не приведет к успеху. Даже если предположить, что у вас уже есть железка, синтезировать систему управления без знаний о ТАУ (а автор, как я понимаю, ими не располагает) - весьма нетривиальная задача.
И скорее всего, как это и бывает обычно в подобных случаях, интерес через некоторое время пропадет, и на этом все закончится.
Советую автору начать с чего-нибудь похожего, но попроще. Например, с перевернутого маятника.
Здесь описан подобный, использующий линейно-квадратичный регулятор. Можно попытаться запилить такой же, но с использованием ПИД регулятора, к примеру.
ИМХО, решение подобной задачи сильно вам поможет. И что самое главное, ее РЕАЛЬНО можно решить, не увязнув в бесконечных математических и инженерных дебрях.