Попробуй сначала простой ПИД-регулятор — измерь крутящий момент, вычти заданное значение, проинтегрируй ошибку, продифференцируй скорость изменения. Только ограничь интегральную составляющую, а то заведется самопроизвольная намотка. Однажды я на долю секунды отвлекся, и мой преобразователь разгонялся как сумасшедший. Если всё равно будет подёргиваться, добавь адаптивный коэффициент усиления, который включается только при пиках шума датчика, и следи за резонансом на 50 герц от редуктора – простой фильтр вырубит эту проблему. Попробуй, подкрути Kp и Ki, а если всё равно будет дергаться, может, датчику нужна новая опора.