Сервопривод из мотора с энкодером на STM32

О сервомоторах

Сервоприводы — незаменимые актуаторы, которые приводят в движение узлы большинства промышленных роботов, узлы автомобилей, самолётов и даже подводных лодок. Самые распространённые DIY-сервомоторы пришли к нам из авиамодельного хобби, это всем известные: SG90, MG995, Futaba 3003. Также есть и специализированные для любительской робототехники приводы: HiWonder LX-16A, Dynamixel AX-12A и подобные им.

Большинство сервоприводов, используемых в любительской среде, умеют контролировать только положение вала. То есть, они могут отклонять вал на заданный угол и удерживать его. Хотя в общем случае, сервомоторы могут контролировать также скорость и крутящий момент.

Как правило, хоббийные сервомоторы состоят из обычного коллекторного двигателя с редуктором, потенциометра для определения угла поворота и небольшой управляющей платы. На этой плате имеется H-мост и микросхема управления.

Работает это устройство по принципу системы с отрицательной обратной связью. В каждый момент времени, система сравнивает целевое значение параметра с текущим и вырабатывает управляющее воздействие, чтобы сравнять их.

В случае сервомотора таким параметром является угол поворота. Целевое значение угла поворота задаёт пользователь с помощью сигнала, передаваемого от микроконтроллера. А текущий угол поворота сервомотор вычисляет при помощи потенциометра, прикреплённого к выходному валу.

В этой статье мы повторим этот принцип и сделаем свой сервомотор, который по точности и надёжности будет лучше тех же SG90 и MG995. Кроме того, мы сможем управлять им при помощи любого интерфейса: I2C, UART, RS485, а не только стандартным ШИМ-сигналом.

Список необходимых компонентов

Для реализации задуманного нам понадобится двигатель с оптическим или магнитным энкодером, драйвер двигателя, а также микроконтроллер с тактовой частотой, достаточной для обработки показаний энкодера.

Примечание

Отладочная плата с микроконтроллером STM32 (STM32F103C8T6)
Программатор ST-LINK V2.0
Драйвер двигателей двухканальный TB6612, I=3.2 А, U=2.5–15 В
Двигатель с редуктором GA12-N20, U=3–6 В, с энкодером
Резистор переменный регулировочный (потенциометр)
Провода вилка-вилка и вилка-розетка, L=10 см
Беспаечная макетная плата

Начнём со сборки испытательного макета, затем настроим в конфигураторе среды CubeIDE всю необходимую периферию и в конце напишем программу с ПИД-регулятором.

Сборка макета

Структуру самодельного сервомотора можно представить в виде 5 блоков:

Структурная схема самодельного сервопривода

Драйвер TB6612 управляет мотором MOT. Энкодер мотора ENC отправляет сигналы в контроллер STM32, который, в свою очередь, управляет драйвером. Наконец, потенциометр POT, которым мы задаём целевой угол, также отправляет сигнал в STM32.

Теперь разберёмся с тем, как все эти элементы между собой связываются.

Подключение двигателя

Двигатель N20 с квадратурным энкодером имеет шесть контактов для подключения.

M2 (красный) — питание мотора;
VCC (чёрный) — питание энкодера;
C2 (жёлтый) — сигнал датчика №1;
C1 (зелёный) — сигнал датчика №2;
GND (синий) — земля энкодера;
M1 (белый) — питание мотора.

Мотор GA12-N20 с энкодером и распиновкой

Контакты мотора M1, M2 подключаем к выходу драйвера TB6612. Полярность на данном этапе не важна.

Всё остальное соединяем с контроллером по схеме:

Мотор N20	VCC	GND	C1	C2
STM32F103C8T6 (bluepill)	3.3	GND	PA8	PA9

Подключение драйвера TB6612

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

К выбору драйвера для сервомотора следует относиться тщательно. Во-первых, драйвер должен обладать достаточным запасом по току, ведь система будет резко стартовать, останавливаться и менять направление вращения мотора. Во-вторых, чем выше допустимая частота ШИМ драйвера, тем более точным будет управление. В-третьих, нужно обеспечить достаточное напряжение на обмотках мотора, то есть драйвер должен быть с запасом по напряжению.

Предупреждение

Использование самого простого L293D приведёт к проблемам, которые потом придётся решать как минимум алгоритмически. При написании данной статьи смена драйвера на TB6612 позволила значительно улучшить качество регулирования.

Таким образом, будем работать именно с модулем на микросхеме TB6612. H-мост собран на транзисторах с МОП-структурой. Частота ШИМ — 100 кГц, вместо 1 кГц у L293D. Максимальный кратковременный ток — 3,2 А.

Используем только один из двух каналов драйвера. Как уже было указано выше, к выходу первого канала драйвера (отмечен M1 или MA) подключаем мотор, а вот остальные контакты соединяем с контроллером по схеме:

Драйвер TB6612	+5V	GND	A1	A2	EA
STM32F103C8T6 (bluepill)	3.3	GND	PA4	PA5	PA6

Совет

Драйвер TB6612 может работать от 3,3 В, поэтому смело запитываем его от STM32.

Подключение потенциометра

Потенциометр нам нужен исключительно для того, чтобы задавать целевой угол. Фактически, мы можем задавать целевой угол напрямую из кода или передавать в систему по какому-нибудь интерфейсу типа I2C или UART. Но для целей отладки потенциометр незаменим!

Тут всё просто, схема знакомая из любого Arduino-урока:

Потенциометр 20 кОм	левый контакт	центр. контакт	правый контакт
STM32F103C8T6 (bluepill)	3.3	PA1	GND

Итоговый макет будет выглядеть примерно так:

Собранный макет самодельного сервопривода

Настройка АЦП, DMA и таймера

Со сборкой макета закончили, приступаем к программированию. Начнём с настройки контактов контроллера в конфигураторе среды. Ниже приведён окончательный вид настроенной периферии.

Окончательный вид настроенной периферии в STM32CubeIDE

В сумме мы должны настроить:

вспомогательные вещи: контакты программатора и тактирование;
АЦП контроллера, чтобы считывать показания потенциометра;
связь АЦП с DMA-контроллером, чтобы не тратить время на опрос АЦП;
запуск АЦП по таймеру;
таймер в режиме опроса энкодера;
ШИМ-выход для управления скоростью мотора;
GPIO для работы с логикой драйвера.

А теперь пройдёмся по каждой настройке отдельно.

Настройка контактов программатора и отладчика

Начнём с настроек контактов программатора. Для этого слева на вкладке Categories переходим в меню System Core, далее в раздел SYS. В поле Debug выбираем Serial Wire.

Теперь контакты PA13 и PA14 будут зарезервированы для программатора. Эта настройка позволит нам забыть про лишние манипуляции при загрузке программы на контроллер в будущем.

Внешнее тактирование

Второй шаг — настройка внешнего тактирования. В меню System Core выбираем RCC и выбираем в поле High Speed Clock (HSE) — кварцевый/керамический резонатор.

Теперь настроим схему тактирования. Разгоним микроконтроллер до 72 МГц.

Со служебными настройками разобрались, переходим к сути.

Настройка АЦП для потенциометра

Согласно нашей схеме, центральный контакт потенциометра мы подключили к ножке PA1. Кликаем по этой ножке в конфигураторе и выбираем режим ADC_IN1.

Заходим в навигаторе в меню Analog и далее в раздел ADC1. Наша задача — запускать АЦП по таймеру с определённой частотой. Ищем в параметрах настройки АЦП пункт External Trigger Conversion Source. Указываем там Timer 2 Capture Compare 2 event.

Теперь АЦП-преобразования будут запускаться каждый раз при наступлении события сравнения на втором канале таймера №2.

Настройка DMA

В действительности, при данных обстоятельствах в DMA нет необходимости. Получать данные из АЦП можно и через опрос (polling), с помощью функции HAL_ADC_PollForConversion. Но в образовательных целях будем разбираться с DMA.

Итак. Не выходя из раздела ADC1, заходим на вкладку DMA Settings и жмём кнопку ADD. Появится раздел с настройками DMA, в котором поменяем только режим на Circular.

Примечание

Размер данных менять не нужно. АЦП у нас 12-разрядное, а значит Half Word (16 разрядов) нам вполне подходит.

Настройка таймера №2

Этот таймер будет не просто запускать преобразования АЦП, он будет задавать темп всей процедуре управления. Частоту его срабатывания можно увеличить или уменьшить, в зависимости от потребностей задачи.

Заходим в меню Timers и далее в раздел TIM2. В поле Clock Source выбираем источник опорной частоты: Internal Clock. Это будет означать, что таймер получит тактирование от шины APB1. Как видно на схеме тактирования, таймеры, привязанные к шине APB1, имеют частоту 72 МГц.

Мы указали в настройках АЦП второй канал данного таймера как источник импульса. Выбираем в поле Channel2 режим Output Compare No Output. Это означает, что на втором канале таймера будут генерироваться сигналы сравнения, но ни на какие контакты они передаваться не будут.

Пусть счётчик таймера осуществляет сравнение с частотой 50 Гц. То есть каждые 20 мс. Для этого в поле Prescaler (предделитель) укажем 7200-1, а в поле Counter Period — 200-1. Таким образом, итоговая частота сравнения будет:

72 000 000 / (7200 * 200) = 50

Нужные нам 50 Гц.

Фуф, с частотами разобрались. Идём дальше.

Настройка ШИМ и энкодера

Таймер в режиме опроса энкодера

Это самое вкусное в STM32. На моторе у нас есть квадратурный магнитный энкодер — два датчика Холла, сдвинутых относительно друг друга так, что фронт одного попадает на середину импульса другого. Картинка покажет суть:

По сути, это обычные прямоугольные импульсы, которые могут быть отлично считаны GPIO микроконтроллера. Мы можем подключить оба канала энкодера к STM32, привязать соответствующие контакты контроллера к прерываниям и следить за фронтами этого сигнала.

Либо мы можем настроить один из таймеров в режиме энкодера и ничего больше не городить. Так и сделаем.

Не выходя из Timers, переключаемся на TIM1. В поле Combined Channels выбираем Encoder Mode.

Настройка таймера TIM1 в режиме энкодера

На вкладке параметров ничего менять не будем. Отметим лишь пару важных параметров.

Counter Period — это максимальное значение счётчика. То есть контроллер сможет отсчитать 65536 фронтов с обоих датчиков Холла, после чего произойдёт переполнение и значение сбросится в 0.

Polarity — фронт, который будет отслеживать таймер. Выбран восходящий — Rising Edge.

Нас эти настройки устраивают, оставляем всё как есть.

Теперь вопрос. На сколько изменится значение счётчика энкодера при полном обороте вала? Чтобы на него ответить, обратимся к спецификации на мотор.

С одной стороны, магнитный диск энкодера имеет 7 полюсов. Когда этот полюс проносится мимо датчика, последний выдаёт импульс. Датчика два, так что будет два импульса на оборот. Таймер контроллера фиксирует передние фронты обоих импульсов и увеличивает счётчик на 2.

С другой стороны, у мотора есть редуктор. В нашем эксперименте участвует мотор с редуктором 1:100. Следовательно, каждый оборот внешнего вала соответствует 100 оборотам магнитного колеса.

Таким образом, один оборот внешнего вала мотора вокруг своей оси даст прирост счётчика на:

7 * 2 * 100 = 1400

Запомним это число.

Настройка ШИМ для управления мотором

Настраиваем ШИМ с максимальной частотой, поэтому в Prescaler указываем 0. Пусть разрешение ШИМ будет 1000 отсчётов (для сравнения, у Arduino — 256). Если не хватит, всегда можем добавить.

Итоговая частота ШИМ будет:

72 000 000 / 1000 = 72 кГц

Опять сравним с Arduino — там будет всего 980 Гц.

Больше ничего не трогаем, переходим к заключительной настройке.

GPIO для работы с логикой драйвера

Для управления направлением вращения мотора мы подключили два контакта A1 и A2 от драйвера к микроконтроллеру на ножки PA4, PA5. Будем работать с ними как с обычными GPIO.

В левом меню переходим в пункт System Core и далее в раздел GPIO. Единственное, что мы можем тут сделать, — это добавить псевдонимы для контактов. Можем назвать их, например, MDIR1 и MDIR2.

С настройкой закончили. Сохраняем конфигурацию, и IDE генерирует код-шаблон для нашей программы.

Программа

После генерации шаблона приступаем к написанию своего кода, строго в выделенных для этого разделах.

Алгоритм работы нашего сервомотора будет таким:

Таймер №2 вызывает событие на 2-м канале. АЦП считывает сигнал с потенциометра, преобразует его в число от 0 до 4095, затем DMA-контроллер копирует это число в заданную ячейку памяти.
Происходит вызов прерывания HAL_ADC_ConvCpltCallback. В этот момент выставляем флаг, что событие свершилось и мы можем работать.
В суперцикле проверяем, что флаг выставлен и вызываем процедуру управления сервомотором.
Получаем текущее значение счётчика энкодера и значение на потенциометре. Формируем управляющий сигнал.
Корректируем ШИМ-сигнал мотора согласно величине управляющего сигнала.

Потенциометр, АЦП и DMA

Чтобы запустить преобразования АЦП с передачей результата в память при помощи DMA, в блоке кода USER CODE 2 пишем нужные вызовы:

/* USER CODE BEGIN 2 */

HAL_ADCEx_Calibration_Start(&hadc1); // калибровка АЦП
HAL_ADC_Start_DMA(&hadc1, (uint32_t*)&pot_adc, 1); // запуск DMA
HAL_TIM_OC_Start(&htim2, TIM_CHANNEL_2); // запуск таймера для опроса АЦП

DMA будет записывать показания потенциометра в переменную pot_adc, которую, неплохо бы, тоже объявить в соответствующем блоке кода. Там же объявим переменную flag, отметив её как volatile. Она пригодится нам далее.

/* USER CODE BEGIN PV */
uint16_t pot_adc;
volatile uint8_t flag = 0;

Далее, нам потребуется переопределить функцию-обработчик прерывания HAL_ADC_ConvCpltCallback. Эта функция будет вызываться каждый раз, когда АЦП порождает новые данные.

/* USER CODE BEGIN 4 */
void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* hadc){
    if(hadc->Instance == ADC1){
        flag = 1;
    }
}
/* USER CODE END 4 */

Важно

В теле обработчика прерывания недопустимы сложные вычисления. Мы лишь изменим значение переменной flag на 1 (не забываем объявить эту переменную).

Далее отредактируем суперцикл в соответствующем блоке.

/* USER CODE BEGIN WHILE */

while (1)
{
    if(flag){
        flag = 0;
        // вызываем процедуру управления
        motorToPos();
    }
/* USER CODE END WHILE */

Смысл такой: как только мы наткнулись на ненулевой flag, обнуляем его и вызываем процедуру управления. Именно в этой процедуре скрывается ПИД-регулятор, который поможет нам правильно вращать мотор.

ШИМ

Ранее мы настроили генерацию ШИМ с помощью таймера №3. Чтобы управлять скоростью вращения мотора, нам нужно будет менять настройки этого таймера, но уже не через конфигуратор, а по ходу программы. Для этого напишем отдельную функцию setPWM.

/* USER CODE BEGIN 4 */
void setPWM(uint16_t value){
    TIM_OC_InitTypeDef sConfigOC;

    HAL_TIM_PWM_Stop(&htim3, TIM_CHANNEL_1);

    sConfigOC.OCMode = TIM_OCMODE_PWM1;
    sConfigOC.Pulse = value;
    sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
    sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;

    HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1);
}

Функция принимает только один аргумент — коэффициент заполнения ШИМ, который у нас от 0 до 1000.

Энкодер

Ещё один таймер обрабатывает сигналы с энкодера. Запустим его, вызвав соответствующую функцию в разделе USER CODE 2:

HAL_TIM_Encoder_Start(&htim1, TIM_CHANNEL_ALL); // запуск энкодера
/* USER CODE END 2 */

Осталось только запрограммировать ПИД-регулятор, чем мы и займёмся в следующей главе.

ПИД-регулятор

Наша задача — сделать сервомотор. Значит, создаваемая нами система должна поворачивать вал мотора на заданный угол и удерживать его. Напомним, целевой угол мы задаём поворотом потенциометра.

Можем попробовать решить эту задачу в лоб, согласно такому наивному алгоритму:

получаем показания потенциометра (pot) и энкодера мотора (enc);
если показания потенциометра меньше счётчика энкодера — крутим мотор в одну сторону;
в противном случае — крутим мотор в другую сторону.

Немного перепишем алгоритм, определив целью минимизацию ошибки. Другими словами, целью работы данного алгоритма будет уменьшение ошибки (error) до нуля.

error = pot - enc;
если error < 0: крутим в сторону уменьшения enc;
иначе: крутим в сторону увеличения enc.

Логика очевидна — просто крутим мотор в нужном направлении, пока не достигнем цели, заданной потенциометром. Однако, запустив программу, мы увидим лишь осциллирующий мотор и перегревающийся драйвер. Вал будет дёргаться в стороны от целевого положения и никогда не остановится.

Чтобы убрать осцилляцию, нужно внести коррективы в алгоритм. Пусть, чем ближе мотор к целевому положению, тем с меньшей скоростью (speed) мы будем его вращать. Чтобы он не перескакивал цель на всех парах.

error = pot - enc;
speed = error * PK;
если error < 0: крутим в сторону уменьшения enc;
иначе: крутим в сторону увеличения enc.

Такой метод управления, когда осуществляется воздействие пропорциональное ошибке, называется пропорциональным управлением. Здесь PK — это коэффициент пропорции, который подбирается исходя из условий задачи, позже с ним разберёмся.

Дальнейшие модификации этого алгоритма рано или поздно приведут нас сначала к появлению I-управления, а затем и D-управления. Все эти типы управления вместе дают полный ПИД-регулятор (PID), который имеет вид:

control = P*KP + I*KI + D*KD

P (пропорциональная часть) — ошибка
I (интегральная часть) — сумма ошибок за все предыдущие итерации
D (дифференциальная часть) — отношение разницы между текущей и предыдущей ошибками к периоду управления

KP, KI, KD — их коэффициенты.

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

Пишем процедуру управления, учитывая знания о ПИД-регуляторе:

/* USER CODE BEGIN 4 */
void motorToPos(){
    // переводим поворот потенциометра в положение выходного вала
    uint16_t pos = pot_adc*POT_SCALE;

    // защита от переполнения счётчика
    // пусть счётчик считает в стороны от нуля: ..., -2, -1, 0 , 1, 2, ...
    int16_t current_pos = __HAL_TIM_GET_COUNTER(&htim1) - 32767;

    // вычисляем ошибку
    int16_t error = pos - current_pos;

    // собираем интеграл
    pid_integral += error;

    // вычисляем величину управляющего воздействия как сумму P, I и D компонентов
    int32_t ctrl = PID_KP * error + PID_KI*pid_integral + PID_KD*(error-pid_error);

    // применяем делитель
    ctrl >>= PID_DIV;

    // сохраняем ошибку для следующей итерации
    pid_error = error;

    // убираем знак у сигнала
    ctrl = ABS( ctrl );

    // накладываем ограничения на вычисленный сигнал
    if( ctrl > PID_MAX ){
        ctrl = PID_MAX;
    }

    // применяем управляющий сигнал к ШИМ
    setPWM( ctrl );

    // устанавливаем направление вращения, в зависимости от ошибки
    if( error > 0 ){
        HAL_GPIO_WritePin(MDIR1_GPIO_Port, MDIR1_Pin, GPIO_PIN_SET);
        HAL_GPIO_WritePin(MDIR2_GPIO_Port, MDIR2_Pin, GPIO_PIN_RESET);
    } else {
        HAL_GPIO_WritePin(MDIR1_GPIO_Port, MDIR1_Pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(MDIR2_GPIO_Port, MDIR2_Pin, GPIO_PIN_SET);
    }
}

Проясним некоторые моменты.

Приведение к одному диапазону

uint16_t pos = pot_adc*POT_SCALE;

Чтобы получить корректную величину ошибки, и уменьшаемое и вычитаемое должны быть приведены к одному диапазону.

Мы уже знаем, что поворот ручки потенциометра после обработки АЦП (pot_adc) даст нам число от 0 до 4095. С другой стороны, счётчик энкодера мотора за оборот даст нам 1400 отсчётов.

Пусть, поворачивая ручку потенциометра от крайнего положения до крайнего, мы ожидаем поворота сервомотора от 0 градусов до 360, то есть полный оборот.

Тогда диапазон АЦП следует сжать в диапазон 1400. Делаем это так:

pos = pot_adc * 1400 / 4095

Вынесем эти константы в раздел defines:

/* USER CODE BEGIN PD */
#define ADC_RES 4095 // разрешение АЦП
#define MOT_WAY 1400 // отсчётов энкодера на один оборот выходного вала

// коэфф. перевода положения потенциометра в положение выходного вала
#define POT_SCALE MOT_WAY/ADC_RES

А в коде управления просто будем умножать pot_adc на коэффициент POT_SCALE.

Борьба с переполнением

Следующая подозрительная строка:

int16_t current_pos = __HAL_TIM_GET_COUNTER(&htim1) - 32767;

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

Предупреждение

Счётчик не может принимать отрицательных значений, а значит, если двигатель повернётся в обратную от начального положения сторону, то мы получим не -1, -2, …, а 65535, 65534, 65533… Что снесёт голову нашему алгоритму.

Чтобы получить годные числа с отрицательной частью, мы при запуске программы установим начальное значение счётчика 32767:

/* USER CODE BEGIN WHILE */

// при пуске установим счётчик в центральное положение
__HAL_TIM_SET_COUNTER(&htim1, 32767);

Делаем это прямо перед суперциклом while.

А затем при каждом вызове процедуры управления будем вычитать из него 32767, как и сделано в коде.

Интегральная часть

Тут всё просто. При каждом расчёте ПИД копим суммарную ошибку:

pid_integral += error;

Дифференциальная часть

Вообще, эта часть должна выглядеть так:

PID_KD*(error-pid_error) * dt

Здесь dt — это время, которое прошло между последними вызовами процедуры управления. В нашем случае это время будет с некоторой погрешностью константой. Поэтому мы можем убрать его из выражения (оно никуда не пропадёт, просто сольётся воедино с коэффициентом KD).

Делитель

Сразу после вычисления управляющего сигнала мы зачем-то делим его на 256.

ctrl >>= PID_DIV;

Так мы избавляемся от медленных операций с вещественными числами. Дело в том, что коэффициенты ПИД-регулятора для нашей задачи без манипуляции с делителем принимают такие значения:

KP = 1.8
KI = 0.01
KD = 0

Чтобы привести эти значения к целым числам, потребуется умножить их на 256.

Затем мы вычисляем управляющий сигнал, подставляя увеличенные в 256 раз коэффициенты. После этого делим полученное значение на 256, возвращая уровень сигнала в исходный диапазон. При этом делитель 256 — это степень двойки, поэтому мы и разделить можем тоже «быстро», с помощью бинарного сдвига вправо >>.

Коэффициенты ПИД

Коэффициенты определим с помощью директивы #define в блоке USER CODE PD:

// коэффициенты PID
#define PID_KP 400
#define PID_KI 3
#define PID_KD 0
#define PID_DIV 8 // деление на 256
/* USER CODE END PD */

В этой статье мы не будем подробно разбираться в особенностях настройки ПИД. Этому посвящено множество статей, инструкций и блогов. Настроим систему до работоспособного состояния, без стремления к оптимальному результату (например, к минимальному времени установления целевого положения).

Совет

Для удобства настройки коэффициентов желательно подключить к макету три дополнительных потенциометра. Тогда не нужно будет выключать систему каждый раз, когда в неё вводятся новые параметры. Но можно обойтись и без них, просто каждый раз перешивая контроллер программой с разными константами.

Совет

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

Алгоритм настройки следующий:

выставляем KP = 0, KI = 0, KD = 0;
увеличиваем KP до тех пор, пока стрелка сервомотора не начнёт перескакивать целевое положение, затем немного убавляем, например: 10, 20, 40, 80, 160, 300, 600 (перескок), 500 (перескок), 400 (отлично);
если стрелка немного не добирается до цели, добавляем коэффициент KI, пока стрелка не начнёт чётко приходить к цели: 1, 2, 4, …;
коэффициент KD оставим равным 0.

Применение управляющего сигнала к ШИМ

Вспомним, что наш управляющий сигнал — это скорость вращения мотора. Напрямую управлять скоростью мы не можем, но зато можем генерировать ШИМ с нужным коэффициентом заполнения (duty cycle). Однако для использования сигнала в качестве ШИМ его нужно немного подготовить.

Во-первых, ПИД-регулятор выдаёт нам управляющий сигнал со знаком: как положительный, так и отрицательный. Но величина ШИМ — сугубо положительная. Убираем знак у сигнала:

ctrl = ABS( ctrl )

Во-вторых, в настройках ШИМ мы задали разрешение ШИМ равным 1000. Значит, итоговый управляющий сигнал должен быть в этом диапазоне. Обрезаем его:

if( ctrl > PID_MAX ){
    ctrl = PID_MAX;
}

Наконец, передаём этот сигнал в процедуру setPWM, которая меняет параметры таймера №3 соответствующим образом.

Направление вращения

В самом конце процедуры управления мы должны приказать драйверу вращать мотор в направлении уменьшения ошибки. Делаем это с помощью функции HAL_GPIO_WritePin:

// устанавливаем направление вращения, в зависимости от ошибки
if( error > 0 ){
    HAL_GPIO_WritePin(MDIR1_GPIO_Port, MDIR1_Pin, GPIO_PIN_SET);
    HAL_GPIO_WritePin(MDIR2_GPIO_Port, MDIR2_Pin, GPIO_PIN_RESET);
} else {
    HAL_GPIO_WritePin(MDIR1_GPIO_Port, MDIR1_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(MDIR2_GPIO_Port, MDIR2_Pin, GPIO_PIN_SET);
}

В общем-то всё. Различных мелочей, типа определения переменных и объявления функций, мы тут не касаемся. Для этого лучше посмотреть полный исходный код программы.

Теперь остаётся загрузить программу и проверить работу сервомотора.