Программа

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

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

  • получаем показания потенциометра (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, KB, 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; 

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

  • КП = 1.8
  • КИ = 0.01
  • КД = 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);
}

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

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


Изменено: