Программа
Наша задача — сделать сервомотор. Значит создаваемая нами система должна поворачивать вал мотора на заданный угол и удерживать его. Напомним, целевой угол мы задаем поворотом потенциометра.
Можем попробовать решить эту задачу в лоб, согласно такому наивному алгоритму:
- получаем показания потенциометра (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);
}
В общем-то всё. Различных мелочей, типа определения переменных и объявления функций мы тут не касаемся. Для этого лучше посмотреть полный исходный код программы.
Теперь остаётся подать загрузить программу и проверить работу сервомотора.