При скорости ротора близкой к нулевой, ЭДС в обмотках можно принебречь и тогда крутящий момент пропорцинален току через ротор если трением в подшипниках тоже принебречь. И еще неравномерностью магнитного поля тоже принебречь. Еще одна фишка в том, что при вращении ротора, щетки коммутируют ток то через N, то через N+1 обмотку. При постоянном подводимом напряжении, ток, соответственно, тоже будет прыгать. Но мы и этим принебрежем.

А дальше все просто. Измеряем отклонение от точки равновесия Xo, умножаем на [отрицательную] константу, выдаем на ШИМ. Это - уравнение эластичной деформации, т.е. пружины. А в чем сложность-то?

Ну, естественно, нужно следить за переполнением переменных, знаками и т.п. чтобы в лоб не получить. Шутки-шутками, а пара приличных ссадин есть. Так что тренироваться лучше на небольших моторах.

Если трение в системе небольшое, то и болтаться палка будет как груз на настоящей пружине - нужно, как минимум, еще демпфер добавить (где уже нужно считать скорость) и критично сбалансировать систему (чтобы выезжала из крайнего отклонения в точку равновесия быстро, но без последующих колебаний.)