А Вы пляшите от требуемого. Что есть ШИМ грубо - изменение момента перевода состояния ноги на противоположное на фиксированном промежутке времени. Т.е. график |____*---|____*---|____*---| Весь период равен Т, интервал до перевода из 0 в 1 равен Х.
Сдесь | - начало отсчета, пусть в этот момент переводим из 1 в 0, * - момент перевода состояния ноги пусть из 0 в 1. Реализовать можно например так - запускаем аппаратный таймер с интервалом работы Т/1000 - он будет отсчитывать тысячные доли периода. По прерыванию таймера инкрементируем 4 счетчика. Сравниваем значения счетчика с величинами в1,в2,в3,в4- коэффициентами модуляции. Если равно- переводим некие ноги из 0 в 1. В конце Т - ноги в 0, счетчики в 0. Все с самого начала.
Как-то так работа видится... Конечно, не без тонких моментов, но дальше сами