57з. Драйвер коллекторного мотора: L298N и TB6612FNG — H-мост, ШИМ на таймере, торможение, энкодер и ПИД скорости

В 57а коллекторный двигатель был уравнениями: пусковой ток 6 А у мотора, который на холостом ходу берёт 0,03 А, H-мост из четырёх ключей, плавный пуск скважностью. В 57б — как мерить его ток шунтом. Здесь — то, что стоит на столе чаще всего: готовый модуль-драйвер на L298N или TB6612FNG, три провода от STM32F103 к нему и энкодер на валу.

Что внутри: какие состояния моста дают входы IN1, IN2 и вход ШИМ; чем биполярная L298N отличается от полевой TB6612FNG; почему одна и та же скважность даёт разную скорость в зависимости от того, что мост делает в паузе ШИМ, — торможение или выбег; как мерить скорость энкодером на таймере и где на малых оборотах кончается разрешение; как держать скорость ПИД-регулятором на целых числах, не накапливая интеграл в насыщении; что происходит при реверсе, при остановленном валу и куда девается энергия тормозящего двигателя.

Опоры: RM0008 (DocID13902 Rev 17): ШИМ и предзагрузка OCxPE, с. 386; CCR больше ARR — выход всё время «1», CCR = 0 — всё время «0», с. 387; режим энкодера и таблица 85, с. 391–392; поле SMS, с. 407; фильтр входа IC1F, с. 414. DocID13587 Rev 17 (STM32F103x8/xB): PA0, PA1 — TIM2_CH1_ETR, TIM2_CH2; PA6, PB0, PB1 — TIM3_CH1, TIM3_CH3, TIM3_CH4, с. 29; PB6, PB7 — TIM4_CH1, TIM4_CH2 с допуском 5 В, с. 32. Документов на L298N и TB6612FNG в проекте нет: таблицы их входов взяты из памяти о документах ST и Toshiba и помечены как непроверенные, чисел из этих документов (падение напряжения, допустимый ток, частота ШИМ) в разделе нет. Двигатель — тот же, что в 57а: 12 В, R = 2 Ом, L = 1 мГн, K = 0,02 В·с/рад, J = 10⁻⁵ кг·м², b = 10⁻⁶ Н·м·с; энкодер условный — 100 штрихов, 400 отсчётов на оборот. Все числа ниже — прогон кода _verify/dcdrv57i/dcdrv57i.c на компьютере (gcc 12.2.0) с моделью моста и двигателя на Python 3.11; прошивка собрана armclang 6.24 под Cortex-M3.

Как это называется в документации

Оригинал	Перевод
brushed DC motor driver, dual H-bridge	драйвер коллекторного двигателя, сдвоенный H-мост
forward, reverse	вперёд, назад
short brake, fast motor stop	торможение накоротко: обмотка замкнута ключами
stop, free running motor stop, coast	выбег: все ключи разомкнуты
standby (STBY)	дежурный режим: выходы отключены
enable input (ENA, ENB)	вход разрешения моста
drive-brake, slow decay	ШИМ с торможением в паузе: медленный спад тока
drive-coast, fast decay	ШИМ с выбегом в паузе: быстрый спад тока через диоды
flyback (freewheeling) diode	обратный диод
encoder interface mode	режим энкодера таймера
counts per revolution (CPR)	отсчётов на оборот
integrator windup, anti-windup	накопление интеграла в насыщении и защита от него
stall current	ток остановленного двигателя
regenerative braking	торможение с возвратом энергии в источник

57з.1 H-мост и четыре состояния

Мост из 57а: два полумоста, двигатель в перемычке между ними. Из шестнадцати сочетаний четырёх ключей полезны четыре, остальные либо повторяют их, либо замыкают питание через одну стойку.

Выбег и торможение — не одно и то же. Разомкнутый мост оставляет току только путь через обратные диоды на шину питания: ток спадает быстро, против полного напряжения, а когда кончится — обмотка отключена и на ней одна ЭДС. Замкнутая накоротко обмотка держит ток, пока его гонит ЭДС, и это тормозит вал. Микросхема-драйвер получает от контроллера не ключи, а входы, и её таблица переводит входы в одно из четырёх состояний.

57з.2 L298N и TB6612FNG

Обе микросхемы — два H-моста в одном корпусе, каждый управляется тремя входами. Различаются ключами и тем, как устроены входы.

Таблица — по памяти о документах ST и Toshiba: в проекте их нет, и перед платой её надо сверить со своим документом. Сколько вольт теряется на ключах L298N и какое сопротивление у ключей TB6612FNG, здесь не сказано по той же причине. Качественно разница такая: у биполярной L298N падение на двух открытых ключах почти не зависит от тока и съедает заметную долю низкого питания, у полевой TB6612FNG оно растёт с током, а на малом токе почти нулевое. Двигатель получает напряжение шины минус это падение, и его скорость холостого хода уменьшается в той же доле.

Главное для прошивки — одна и та же пауза ШИМ на разных входах значит разное. Код моста dc_pins знает это про обе микросхемы:

void dc_pins(uint8_t chip, uint8_t mode, int32_t duty, uint16_t period, dc_pins_t *o)
{
    uint16_t on = (uint16_t)(duty < 0 ? (duty < -(int32_t)period ? period : -duty)
                                      : (duty > (int32_t)period ? period : duty));
    uint16_t a = duty >= 0 ? period : 0, b = duty < 0 ? period : 0;
    if ((chip == DC_TB6612) == (mode == DC_DRIVE_BRAKE)) {
        o->pwm = on;                                    /* ШИМ на входе PWM (ENA), направление — уровнями */
        o->in1 = a;
        o->in2 = b;
    } else {
        o->pwm = period;                                /* PWM (ENA) всегда «1», ШИМ — на входе направления */
        o->in1 = duty >= 0 ? on : 0;
        o->in2 = duty < 0 ? on : 0;
    }
}

Все три вывода — каналы одного таймера, и значение каждого — CCR: 0 — всё время «0», period = ARR + 1 — всё время «1» (RM0008, с. 387), между ними — ШИМ. Модель стенда переводит выводы в состояния моста по своим таблицам обеих микросхем. При скважности 900 из 3600 (25 %) вперёд:

У TB6612FNG торможение в паузе — ШИМ на входе PWM, у L298N — на входе IN. Для всех пяти проверенных скважностей (−3600, −900, 0, 900, 3600) и обоих режимов мост обеих микросхем проходит одни и те же состояния, и скорость двигателя в модели совпадает. Скважность 0 в режиме торможения — торможение, в режиме выбега — выбег.

57з.3 ШИМ на таймере: частота, торможение и выбег

На F103 удобно взять TIM3: PA6, PB0 и PB1 — его каналы 1, 3 и 4 (DocID13587, с. 29). TIM3 сидит на APB1, но считает от 72 МГц (раздел 13.3): PSC = 0, ARR = 3599 — ШИМ 20 кГц и 3600 ступеней. TIM1 тоже годится — у него ещё бит MOE (13.5), а дополняющие выходы с защитным интервалом (57а.6) нужны мосту из отдельных транзисторов, не модулю. Все три канала — с предзагрузкой (RM0008, с. 386): новые CCR вступают вместе на одном обновлении, и вход направления не переключится раньше входа ШИМ.

void SysTick_Handler(void)
{
    dc_pins_t o;
    ms++;
    dc_ctl_tick(&ctl, (uint16_t)TIM2->CNT, &o);
    TIM3->CCR1 = o.pwm;                                 /* все три — в буферы: вступят вместе */
    TIM3->CCR3 = o.in1;                                 /* на одном обновлении TIM3 */
    TIM3->CCR4 = o.in2;
}

Частота ШИМ. Электрическая постоянная обмотки — L / R = 0,5 мс, и всё решает, сколько периодов ШИМ в неё укладывается. Модель: скважность 50 %, нагрузка 0,01 Н·м (ток 0,5 А), 5 с до установившегося режима, средние — за последние 0,1 с:

«Потери в меди» — отношение квадрата действующего тока к квадрату среднего: во сколько раз пульсации греют обмотку сильнее постоянного тока той же силы. При 1 кГц — в 3,6 раза, при 20 кГц — почти без лишнего. Кроме того, 1 и 4 кГц — посреди слышимого диапазона: обмотка и корпус пищат на частоте ШИМ; 20 кГц — у верхней границы слуха. Платят за высокую частоту потерями на переключение ключей: они растут пропорционально частоте, а сколько их у L298N и TB6612FNG — в документах, которых в проекте нет.

Торможение в паузе. Ток не прерывается, и на обмотке в среднем ровно скважность × U: 6,00 В при 50 % на любой частоте, и скорость одна и та же. Без нагрузки скорость пропорциональна скважности — 570, 1425, 2851, 4276 и 5131 об/мин при 10, 25, 50, 75 и 90 %, ровно доли от 5701 об/мин холостого хода (57а).

Выбег в паузе. Пока ток течёт через диоды, на обмотке −U, и он спадает быстро. На 20 кГц ток за паузу успевает упасть до нуля — он прерывистый, и его среднего не хватает на нагрузку. Под той же нагрузкой 0,01 Н·м, 20 кГц:

До 50 % в режиме выбега двигатель не трогается. С 75 % ток уже не прерывается: часть периода на обмотке +U, остальное −U, в среднем (2d − 1)·U — при 75 % те же 6,00 В и те же 2375 об/мин, что торможение даёт при 50 %. А на низкой частоте выбег, наоборот, разгоняет сильнее: при 1 кГц ток кончается рано, обмотка остаток паузы разомкнута, и на ней ЭДС — в среднем 8,05 В вместо 6. Скорость в режиме выбега зависит от частоты ШИМ и нагрузки, в режиме торможения — нет. Для регулятора скорости берут торможение в паузе.

57з.4 Энкодер: скорость из CNT

Режим энкодера — раздел 13.7: TIM2 с SMS = 011 считает оба фронта обоих каналов (RM0008, с. 391 и 407), направление определяет сам по таблице 85 (с. 392). Энкодер на валу двигателя в 100 штрихов даёт 400 отсчётов на оборот. Вход — с фильтром IC1F = 0011: 8 отсчётов на 72 МГц (с. 414); у энкодера двигателя фронты идут на высокой скорости часто, и длинный фильтр ручки из 13.10 здесь не годится.

Скорость — разность показаний CNT со знаком, сумма за окно из нескольких тиков регулятора:

int32_t enc_speed(enc_t *e, uint16_t cnt)
{
    int16_t d = (int16_t)(uint16_t)(cnt - e->last);   /* разность со знаком: переход через 0 не страшен */
    e->last = cnt;
    e->sum += d - e->d[e->k];
    e->d[e->k] = d;
    if (++e->k >= e->win)
        e->k = 0;
    return e->sum * (int32_t)CTL_HZ / e->win;
}

Разрешение такой скорости — один отсчёт за окно: при тике 1 мс и окне в один тик это 1000 отсчётов/с, 150 об/мин. Модель вращает вал с постоянной скоростью 2 с и смотрит, что показывает enc_speed:

Ступень постоянна, и на малых оборотах она становится всей величиной: с окном 1 мс скорость 100 об/мин показана то нулём, то 150. Длинное окно даёт мелкую ступень, но запаздывает — скорость за 32 мс отстаёт от настоящей примерно на половину окна. Регулятору раздела хватает 8 мс. Если нужно точнее на малых оборотах, мерят не число отсчётов за время, а время между отсчётами — захватом (13.6); здесь этого нет.

Переход CNT через 65 535 разность со знаком не замечает: 10 с на 3000 об/мин — 200 000 отсчётов, три перехода, и ни один тик не показал скорость дальше одной ступени от 20 000 отсчётов/с.

57з.5 ПИД скорости на целых числах и рампа

Регулятор — из 47.6, только в шагах CCR: ошибка в отсчётах в секунду, коэффициенты в Q16, выход — скважность со знаком в пределах ±3600. Производная берётся по измерению, но в прошивке kd = 0: скорость с энкодера ступенчатая, и производная от ступеней — шум (47.4).

int32_t pid57_step(pid57_t *p, int32_t sp, int32_t meas)
{
    int32_t e = sp - meas;
    int64_t pd = (int64_t)p->kp * e - (int64_t)p->kd * (meas - p->prev);
    p->prev = meas;
    int64_t u = (pd + p->integ) / 65536;
    int64_t lim = p->out_max;
    /* насыщение: выход уже в пределе, и ошибка гонит его дальше — интеграл не копим */
    if (!p->aw || !((u >= lim && e > 0) || (u <= -lim && e < 0)))
        p->integ = (int32_t)clamp64((int64_t)p->integ + (int64_t)p->ki * e, p->integ_max);
    return (int32_t)clamp64((pd + p->integ) / 65536, lim);
}

Защиты от накопления две: предел самого интеграла (integ_max — полный выход) и запрет копить, пока выход упёрся в предел и ошибка гонит его дальше. В прошивке kp = 15000 (0,229 шага CCR на отсчёт/с) и ki = 300 на тик. Задание идёт к цели рампой — не быстрее 40 отсчётов/с за тик, это 40 000 отсчётов/с²: до 3000 об/мин (20 000 отсчётов/с) за 0,5 с.

Прогон: цель 3000 об/мин с рампой, через 1 с — нагрузка 0,01 Н·м:

Без рампы и без защиты. Цель 3000 об/мин сразу: регулятор упирается в предел, двигатель пускается от полного напряжения, ток — 5,76 А, почти пусковой ток из 57а. С запретом копить интеграл в насыщении выход стоит в пределе 11 мс и перелёт 4,2 об/мин (0,1 %); без него — 22 мс в пределе, интеграл набирает лишнее и перелёт 52,8 об/мин (1,8 %). Ближе к пределу двигателя разница больше: цель 5000 об/мин под нагрузкой — 43 мс в пределе и 1,8 об/мин перелёта против 159 мс и 56,2 об/мин (1,1 %).

Малые обороты. Цель 100 об/мин: с окном 8 мс скорость вала ходит в пределах 3,3 об/мин, скважность — в пределах 31 шага; с окном 32 мс — 2,6 об/мин и 9 шагов. Ступенька измерения переходит в дрожание скважности.

Весь тик регулятора — скорость, рампа, ПИД, защита и мост:

void dc_ctl_tick(dc_ctl_t *c, uint16_t cnt, dc_pins_t *o)
{
    c->speed = enc_speed(&c->enc, cnt);
    if (c->fault) {
        c->duty = 0;
        dc_state(c->drv.chip, DC_COAST, c->drv.period, o);
        return;
    }
    c->sp = ramp_step(c->sp, c->target, c->accel);
    int32_t u = pid57_step(&c->pid, c->sp, c->speed);

57з.6 Защита: реверс, ток останова, рекуперация, питание

Реверс. При смене знака dc_drive держит мост в выбеге dead тиков (в прошивке — 1 мс), чтобы ток прежнего направления спал через диоды. Модель показывает, что от броска тока это не спасает: двигатель на 3000 об/мин за 1 мс почти не замедлился, и обратное напряжение складывается с его ЭДС. Цель с +3000 сразу на −3000 об/мин:

Без рампы ток при реверсе больше пускового (6 А): обмотка получает U плюс ЭДС. Пауза только сдвигает бросок на миллисекунду. Мёртвое время нужно другому — чтобы два ключа одной стойки не были открыты одновременно; в L298N и TB6612FNG его обеспечивает сама микросхема (по документам, которых в проекте нет), а мосту из отдельных транзисторов — DTG таймера TIM1 (57а.6). Бросок тока при реверсе убирает рампа: с ней пик −0,41 А.

Рекуперация. С рампой двигатель тормозится током обратного знака, и в режиме торможения в паузе ток от шины становится отрицательным — до −0,41 А: двигатель работает генератором и возвращает энергию. В модели до остановки шина получила обратно 0,381 Дж из 0,493 Дж, запасённых в роторе на 3000 об/мин; остальное — тепло в обмотке. Без рампы наоборот: шина отдаёт ещё 1,885 Дж, и всё уходит в тепло. Вернувшуюся энергию кто-то должен принять: аккумулятор примет, а блок питания с диодом на выходе — нет, и напряжение на конденсаторе шины растёт. Насколько — зависит от ёмкости и источника; здесь это не считалось. Помогают рампа торможения, конденсатор у модуля и, на больших двигателях, ограничитель напряжения шины.

Ток останова. Заклинило вал — регулятор видит ошибку и поднимает скважность до упора, а ток идёт к U / R = 6 А и стоит там, пока не сгорит обмотка или драйвер. Защита в dc_ctl_tick: выход не меньше 90 % предела, а вал медленнее 30 об/мин 200 тиков подряд — мост в выбег до сброса.

    int32_t au = u < 0 ? -u : u, as = c->speed < 0 ? -c->speed : c->speed;
    /* выход почти в пределе, а вал стоит: ток останова греет обмотку и ключи — снять питание */
    if (au >= c->pid.out_max * 9 / 10 && as < c->stall_speed) {
        if (++c->stall_n >= c->stall_ms) {
            c->fault = 1;
            c->duty = 0;
            dc_state(c->drv.chip, DC_COAST, c->drv.period, o);
            return;
        }
    } else {
        c->stall_n = 0;
    }

Модель на 1000 об/мин заклинивает вал: защита срабатывает через 238 мс (регулятору нужно время, чтобы дойти до 90 %, плюс 200 тиков), ток за это время в среднем 5,63 А, пик 6,00 А, потом 0. Без защиты через 0,5 с скважность 3600 из 3600 и 6,00 А постоянно. Это защита по скорости, а не по току: ток так не ограничить, его мерят шунтом (57б).

Питание и земля. Логика драйвера питается от 3,3 или 5 В, двигатель — от своей шины; минус шины, минус логики и земля STM32 соединены, иначе у входов нет общего нуля. Импульсный ток двигателя не должен идти по земле контроллера: силовой минус — толстым проводом к модулю, к контроллеру — отдельный. У модуля — конденсатор по шине мотора. Допустимые уровни входов и напряжения питания — в документе своей микросхемы.

57з.7 Грабли

Мотор крутится на малой скважности рывками или стоит, хотя «ШИМ есть». Пауза ШИМ попала на вход, который даёт выбег: у TB6612FNG это ШИМ на IN, у L298N — на ENA. На 20 кГц под нагрузкой 0,01 Н·м двигатель в выбеге не трогается до 50 %. Режим торможения — ШИМ на PWM у TB6612FNG и на IN у L298N.

Скорость меняется с частотой ШИМ. Тот же выбег в паузе: 3348 об/мин на 1 кГц и 580 на 4 кГц при одной скважности 50 %. С торможением в паузе — 2374…2375 на любой частоте.

Мотор пищит. ШИМ 1 или 4 кГц — посреди слышимого диапазона. 20 кГц (ARR = 3599 на 72 МГц) у верхней границы слуха, и пульсации тока меньше: 0,150 А против 2,774 А на 1 кГц.

Скорость прыгает 0 — 150 об/мин. Окно измерения в один тик: ступень 150 об/мин при 400 отсчётах на оборот. Окно 8 мс — ступень 18,75.

После перехода CNT через 65 535 скорость скачет. Берут сам CNT, а не разность со знаком (int16_t)(uint16_t)(cnt - last).

Энкодер от 5 В на PA0, PA1. У этих выводов нет допуска 5 В (DocID13587, с. 29): энкодер с выходами на 5 В — на PB6, PB7 (TIM4, допуск 5 В, с. 32) или через делитель; с открытым коллектором — с подтяжкой к 3,3 В.

Перелёт после разгона. Интеграл копился, пока выход стоял в пределе: 52,8 об/мин перелёта против 4,2 с запретом копить в насыщении.

Драйвер уходит в защиту или греется при реверсе. Цель меняется скачком: −8,81 А при реверсе с 3000 об/мин. Пауза в выбеге не помогает, помогает рампа: −0,41 А.

При торможении растёт напряжение шины. Двигатель с рампой возвращает энергию в источник; блок питания с диодом её не примет. Конденсатор, ограничитель или аккумулятор.

Вал заклинило — драйвер горячий. Регулятор держит 100 % и 6 А. Защита по скорости: выход в пределе, а вал стоит, — выбег.

Мотор не крутится, хотя всё настроено. TB6612FNG в дежурном режиме: STBY = 0. Или не соединены земли модуля и контроллера.

57з.8 Итоги

• H-мост: вперёд, назад, торможение накоротко (обмотка замкнута), выбег (все ключи разомкнуты, ток через диоды на шину).
• L298N — биполярные ключи, ENA = 0 — выбег; TB6612FNG — полевые, PWM = 0 — торможение, есть STBY. Одна пауза ШИМ на разных входах значит разное: код моста держит таблицу для обеих.
• ШИМ — три канала TIM3 (PA6, PB0, PB1) с предзагрузкой, ARR = 3599 — 20 кГц; пульсации тока 0,150 А против 2,774 А на 1 кГц, лишние потери в меди 1,007 против 3,599.
• Торможение в паузе: среднее напряжение = скважность × U, скорость не зависит от частоты (2375 об/мин при 50 %). Выбег в паузе: ток прерывистый, на 20 кГц под нагрузкой двигатель стоит до 50 %.
• Энкодер на TIM2, 400 отсчётов на оборот: ступень скорости 150 об/мин с окном 1 мс, 18,75 — с окном 8 мс, 4,69 — с окном 32 мс.
• ПИД в Q16 с рампой 40 отсчётов/с за тик: 3000 об/мин за 507 мс, провал при нагрузке до 2880,4 об/мин и 76 мс на восстановление; запрет копить интеграл в насыщении — перелёт 4,2 об/мин вместо 52,8.
• Реверс без рампы — −8,81 А, пауза 1 мс его не меняет; с рампой −0,41 А, и шина получает обратно 0,381 Дж из 0,493.
• Останов вала: 6 А без защиты; защита по скорости снимает питание через 238 мс.

Полные исходники

dcdrv57i.h — мост, энкодер, ПИД и регулятор: типы и прототипы.

/* dcdrv57i.h — раздел 57з: коллекторный двигатель через модуль-драйвер, без регистров.
 * Мост (L298N или TB6612FNG) управляется тремя выводами: два входа направления и вход ШИМ
 * (у L298N — ENA). Все три — каналы одного таймера, значение каждого — CCR: 0 — всегда «0»,
 * period — всегда «1», между ними — ШИМ. Код один для прошивки fw57i.c и стенда host57i.c. */
#ifndef DCDRV57I_H
#define DCDRV57I_H
#include <stdint.h>

#define DC_PERIOD   3600u       /* 72 МГц / 20 кГц: ARR = 3599 */
#define CTL_HZ      1000u       /* регулятор — раз в 1 мс */
#define ENC_CPR     400         /* отсчётов на оборот вала: энкодер 100 штрихов × 4 фронта */
#define ENC_WIN_MAX 32u         /* самое длинное окно измерения скорости, тиков регулятора */
#define RPM(x)      ((int32_t)(x) * ENC_CPR / 60)       /* об/мин → отсчётов в секунду */

enum { DC_COAST = 0, DC_FWD, DC_REV, DC_BRAKE };        /* состояние моста */
enum { DC_TB6612 = 0, DC_L298N };                       /* микросхема */
enum { DC_DRIVE_BRAKE = 0, DC_DRIVE_COAST };            /* что в паузе ШИМ: торможение или выбег */

typedef struct { uint16_t pwm, in1, in2; } dc_pins_t;   /* CCR выводов PWM (ENA), IN1, IN2 */

typedef struct {
    uint8_t chip, mode;
    uint16_t period;            /* ARR + 1 */
    uint16_t dead;              /* тиков в выбеге при смене направления */
    uint16_t wait;              /* сколько ещё ждать */
    int8_t dir;                 /* −1, 0, +1: последнее направление */
} dc_drv_t;

typedef struct {
    uint16_t last;              /* прошлое показание CNT */
    uint8_t win, k;             /* окно и место в кольце */
    int16_t d[ENC_WIN_MAX];     /* приращения за последние win тиков */
    int32_t sum;
} enc_t;

typedef struct {
    int32_t kp, ki, kd;         /* Q16: шагов CCR на (отсчёт/с) */
    int32_t integ, integ_max;   /* Q16, накопитель и его предел */
    int32_t prev;               /* прошлое измерение: производная — по нему */
    int32_t out_max;            /* предел выхода: ±period */
    uint8_t aw;                 /* 1 — не копить интеграл, пока выход в пределе */
} pid57_t;

typedef struct {
    dc_drv_t drv;
    enc_t enc;
    pid57_t pid;
    int32_t target, sp, accel;  /* цель, задание после рампы, шаг рампы за тик (отсчётов/с) */
    int32_t speed, duty;        /* измерено и подано */
    int32_t stall_speed;        /* «стоит», если медленнее, отсчётов/с */
    uint16_t stall_ms, stall_n; /* сколько тиков подряд терпеть и сколько уже */
    uint8_t fault;              /* 1 — останов: мост в выбеге до сброса */
} dc_ctl_t;

void dc_state(uint8_t chip, uint8_t st, uint16_t period, dc_pins_t *o);
void dc_pins(uint8_t chip, uint8_t mode, int32_t duty, uint16_t period, dc_pins_t *o);
int32_t dc_drive(dc_drv_t *d, int32_t duty, dc_pins_t *o);
void enc_init(enc_t *e, uint8_t win, uint16_t cnt);
int32_t enc_speed(enc_t *e, uint16_t cnt);
int32_t pid57_step(pid57_t *p, int32_t sp, int32_t meas);
int32_t ramp_step(int32_t sp, int32_t target, int32_t step);
void dc_ctl_tick(dc_ctl_t *c, uint16_t cnt, dc_pins_t *o);

#endif

dcdrv57i.c — код моста, скорости по энкодеру, ПИД, рампы и защиты — общий для прошивки и стенда.

/* dcdrv57i.c — раздел 57з: мост, энкодер, рампа и ПИД скорости на целых числах. */
#include "dcdrv57i.h"

/* Мост стоит: выбег — все ключи разомкнуты, торможение — обмотка замкнута накоротко. */
void dc_state(uint8_t chip, uint8_t st, uint16_t period, dc_pins_t *o)
{
    if (chip == DC_L298N) {
        o->pwm = st == DC_BRAKE ? period : 0;          /* ENA = 0 — выбег; ENA = 1, IN1 = IN2 — торможение */
        o->in1 = o->in2 = 0;
    } else {
        o->pwm = period;                                /* TB6612: IN1 = IN2 = 0 — выбег, IN1 = IN2 = 1 — торможение */
        o->in1 = o->in2 = st == DC_BRAKE ? period : 0;
    }
}

/* Скважность со знаком: duty = ±period — полное напряжение, 0 — пауза всё время.
 * В паузе ШИМ мост тормозит (DC_DRIVE_BRAKE) или выбегает (DC_DRIVE_COAST). У TB6612 пауза на входе
 * PWM — торможение, на входе IN — выбег; у L298N наоборот: пауза на ENA — выбег, на IN — торможение. */
void dc_pins(uint8_t chip, uint8_t mode, int32_t duty, uint16_t period, dc_pins_t *o)
{
    uint16_t on = (uint16_t)(duty < 0 ? (duty < -(int32_t)period ? period : -duty)
                                      : (duty > (int32_t)period ? period : duty));
    uint16_t a = duty >= 0 ? period : 0, b = duty < 0 ? period : 0;
    if ((chip == DC_TB6612) == (mode == DC_DRIVE_BRAKE)) {
        o->pwm = on;                                    /* ШИМ на входе PWM (ENA), направление — уровнями */
        o->in1 = a;
        o->in2 = b;
    } else {
        o->pwm = period;                                /* PWM (ENA) всегда «1», ШИМ — на входе направления */
        o->in1 = duty >= 0 ? on : 0;
        o->in2 = duty < 0 ? on : 0;
    }
}

/* Смена знака — через выбег на d->dead тиков: ток прошлого направления спадает через диоды,
 * а не встречает сразу противоположную диагональ. Возвращает поданную скважность. */
int32_t dc_drive(dc_drv_t *d, int32_t duty, dc_pins_t *o)
{
    int8_t s = (int8_t)(duty > 0 ? 1 : duty < 0 ? -1 : 0);
    if (s != 0 && d->dir != 0 && s != d->dir) {
        d->wait = d->dead;
        d->dir = 0;
    }
    if (d->wait) {
        d->wait--;
        dc_state(d->chip, DC_COAST, d->period, o);
        return 0;
    }
    if (s != 0)
        d->dir = s;
    dc_pins(d->chip, d->mode, duty, d->period, o);
    return duty;
}

void enc_init(enc_t *e, uint8_t win, uint16_t cnt)
{
    e->last = cnt;
    e->win = win < 1u ? 1u : win > ENC_WIN_MAX ? ENC_WIN_MAX : win;
    e->k = 0;
    e->sum = 0;
    for (unsigned i = 0; i < ENC_WIN_MAX; i++)
        e->d[i] = 0;
}

/* Скорость в отсчётах в секунду: сумма приращений за окно из win тиков. */
int32_t enc_speed(enc_t *e, uint16_t cnt)
{
    int16_t d = (int16_t)(uint16_t)(cnt - e->last);   /* разность со знаком: переход через 0 не страшен */
    e->last = cnt;
    e->sum += d - e->d[e->k];
    e->d[e->k] = d;
    if (++e->k >= e->win)
        e->k = 0;
    return e->sum * (int32_t)CTL_HZ / e->win;
}

static int64_t clamp64(int64_t x, int64_t lim)
{
    return x > lim ? lim : x < -lim ? -lim : x;
}

/* ПИД на Q16 (раздел 47.6): интеграл со своим пределом, производная по измерению, выход — в шагах CCR. */
int32_t pid57_step(pid57_t *p, int32_t sp, int32_t meas)
{
    int32_t e = sp - meas;
    int64_t pd = (int64_t)p->kp * e - (int64_t)p->kd * (meas - p->prev);
    p->prev = meas;
    int64_t u = (pd + p->integ) / 65536;
    int64_t lim = p->out_max;
    /* насыщение: выход уже в пределе, и ошибка гонит его дальше — интеграл не копим */
    if (!p->aw || !((u >= lim && e > 0) || (u <= -lim && e < 0)))
        p->integ = (int32_t)clamp64((int64_t)p->integ + (int64_t)p->ki * e, p->integ_max);
    return (int32_t)clamp64((pd + p->integ) / 65536, lim);
}

/* Рампа: задание идёт к цели не быстрее step за тик. */
int32_t ramp_step(int32_t sp, int32_t target, int32_t step)
{
    if (target > sp + step)
        return sp + step;
    if (target < sp - step)
        return sp - step;
    return target;
}

/* Тик регулятора раз в 1 мс: скорость по энкодеру, рампа, ПИД, защита от останова, мост. */
void dc_ctl_tick(dc_ctl_t *c, uint16_t cnt, dc_pins_t *o)
{
    c->speed = enc_speed(&c->enc, cnt);
    if (c->fault) {
        c->duty = 0;
        dc_state(c->drv.chip, DC_COAST, c->drv.period, o);
        return;
    }
    c->sp = ramp_step(c->sp, c->target, c->accel);
    int32_t u = pid57_step(&c->pid, c->sp, c->speed);
    int32_t au = u < 0 ? -u : u, as = c->speed < 0 ? -c->speed : c->speed;
    /* выход почти в пределе, а вал стоит: ток останова греет обмотку и ключи — снять питание */
    if (au >= c->pid.out_max * 9 / 10 && as < c->stall_speed) {
        if (++c->stall_n >= c->stall_ms) {
            c->fault = 1;
            c->duty = 0;
            dc_state(c->drv.chip, DC_COAST, c->drv.period, o);
            return;
        }
    } else {
        c->stall_n = 0;
    }
    c->duty = dc_drive(&c->drv, u, o);
}

fw57i.c — прошивка STM32F103: TIM3 — ШИМ на три входа драйвера, TIM2 — энкодер, регулятор в SysTick.

/* fw57i.c — раздел 57з: коллекторный двигатель через TB6612FNG (или L298N) на STM32F103 (BluePill), 72 МГц.
 * TIM3 — ШИМ 20 кГц на трёх каналах: PA6 (CH1) — PWMA (у L298N — ENA), PB0 (CH3) — AIN1 (IN1), PB1 (CH4) — AIN2 (IN2).
 * TIM2 — энкодер на PA0, PA1, счёт по обоим фронтам. PB12 — STBY у TB6612 (у L298N не нужен).
 * SysTick раз в 1 мс: скорость, рампа, ПИД, защита от останова (dcdrv57i.c), новые CCR — с ближайшего обновления.
 * Тактирование до 72 МГц — как в разделе 6; TIM2 и TIM3 на APB1 (36 МГц) считают от 72 МГц (раздел 13.3). */
#include "stm32f10x.h"
#include "dcdrv57i.h"

#define CPU_HZ   72000000u
#define DRV_CHIP DC_TB6612
#define DRV_MODE DC_DRIVE_BRAKE

_Static_assert(CPU_HZ / DC_PERIOD == 20000u, "ШИМ 20 кГц: ARR = 3599");

static dc_ctl_t ctl;
static volatile uint32_t ms;

void SysTick_Handler(void)
{
    dc_pins_t o;
    ms++;
    dc_ctl_tick(&ctl, (uint16_t)TIM2->CNT, &o);
    TIM3->CCR1 = o.pwm;                                 /* все три — в буферы: вступят вместе */
    TIM3->CCR3 = o.in1;                                 /* на одном обновлении TIM3 */
    TIM3->CCR4 = o.in2;
}

static void pwm_init(void)
{
    GPIOA->CRL = (GPIOA->CRL & ~(0xFu << 24)) | 0xBu << 24;    /* PA6 — TIM3_CH1, push-pull */
    GPIOB->CRL = (GPIOB->CRL & ~0xFFu) | 0xBBu;                /* PB0, PB1 — TIM3_CH3, TIM3_CH4 */
    TIM3->PSC = 0;
    TIM3->ARR = DC_PERIOD - 1u;
    TIM3->CCMR1 = TIM_CCMR1_OC1M_2 | TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1PE;      /* ШИМ 1, буфер */
    TIM3->CCMR2 = TIM_CCMR2_OC3M_2 | TIM_CCMR2_OC3M_1 | TIM_CCMR2_OC3PE
                | TIM_CCMR2_OC4M_2 | TIM_CCMR2_OC4M_1 | TIM_CCMR2_OC4PE;
    TIM3->CCER = TIM_CCER_CC1E | TIM_CCER_CC3E | TIM_CCER_CC4E;
    dc_pins_t o;
    dc_state(DRV_CHIP, DC_COAST, DC_PERIOD, &o);        /* до первого тика — выбег */
    TIM3->CCR1 = o.pwm;
    TIM3->CCR3 = o.in1;
    TIM3->CCR4 = o.in2;
    TIM3->CR1 = TIM_CR1_ARPE;
    TIM3->EGR = TIM_EGR_UG;
    TIM3->CR1 |= TIM_CR1_CEN;
}

static void enc_tim_init(void)
{
    /* PA0, PA1 — входы с подтяжкой вверх: у энкодера с открытым коллектором своей нет */
    GPIOA->CRL = (GPIOA->CRL & ~0xFFu) | 0x88u;
    GPIOA->BSRR = GPIO_BSRR_BS0 | GPIO_BSRR_BS1;
    TIM2->CCMR1 = TIM_CCMR1_CC1S_0 | TIM_CCMR1_CC2S_0                        /* TI1, TI2 — входы */
                | TIM_CCMR1_IC1F_1 | TIM_CCMR1_IC1F_0 | TIM_CCMR1_IC2F_1 | TIM_CCMR1_IC2F_0;   /* 0011: N = 8 */
    TIM2->SMCR = TIM_SMCR_SMS_1 | TIM_SMCR_SMS_0;       /* 011: энкодер, оба фронта обоих каналов */
    TIM2->ARR = 0xFFFFu;
    TIM2->CR1 = TIM_CR1_CEN;
}

int main(void)
{
    RCC->APB2ENR |= RCC_APB2ENR_IOPAEN | RCC_APB2ENR_IOPBEN | RCC_APB2ENR_IOPCEN;
    RCC->APB1ENR |= RCC_APB1ENR_TIM2EN | RCC_APB1ENR_TIM3EN;
    GPIOB->CRH = (GPIOB->CRH & ~(0xFu << 16)) | 0x2u << 16;    /* PB12 — STBY */
    GPIOC->CRH = (GPIOC->CRH & ~(0xFu << 20)) | 0x2u << 20;    /* PC13 — светодиод платы */
    pwm_init();
    enc_tim_init();

    ctl.drv = (dc_drv_t){DRV_CHIP, DRV_MODE, DC_PERIOD, 1u, 0u, 0};
    enc_init(&ctl.enc, 8u, (uint16_t)TIM2->CNT);
    ctl.pid = (pid57_t){15000, 300, 0, 0, 3600 << 16, 0, DC_PERIOD, 1u};
    ctl.accel = 40;                                     /* 40 отсчётов/с за 1 мс: 0 → 3000 об/мин за 0,5 с */
    ctl.stall_speed = RPM(30);
    ctl.stall_ms = 200u;
    ctl.target = RPM(1500);
    GPIOB->BSRR = GPIO_BSRR_BS12;                       /* TB6612 из дежурного режима */
    SysTick_Config(CPU_HZ / CTL_HZ);

    uint32_t next = 0;
    for (;;) {
        if ((int32_t)(ms - next) >= 0) {
            next += 3000u;                              /* каждые 3 с — реверс: +1500 ↔ −1500 об/мин */
            __disable_irq();
            ctl.target = -ctl.target;
            __enable_irq();
        }
        GPIOC->BSRR = ctl.fault ? GPIO_BSRR_BR13 : GPIO_BSRR_BS13;   /* светодиод горит — останов */
    }
}

Дальше: раздел 57и «Магнитный энкодер AS5600: угол по I2C и ШИМ, нулевая точка, счёт оборотов и скорость» — от счёта приращений к абсолютному углу: магнит на валу, датчики Холла по I2C и обороты без потери нуля.