Модуль для регулирования коллекторного двигателя с энкодером.
- Использование одного/двух/трех контуров управления (по положению, угловой скорости и току)
- Регулирование как положения, так и угловой скорости (при наличии контура скорости)
- Встроен алгоритм управления каналом таймера в режиме генерации ШИМ, а также выбором направления вращения мотора с помощью двух GPIO
- Встроен опрос энкодера, расчет относительного положения и угловой скорости
Внимание: данные для контура тока необходимо вводить самостоятельно, т.е. вам нужно самим настроить АЦП или получение данных о токе из внешнего источника
- Установка
- Настройка проекта
- Объявление экземпляра структуры
- Инициализация
- Опрос данных и расчёт контуров регулирования
- Регулирование
- Прочий функционал
- Пример
Распаковать архив _servocontrol.zip из релиза в папку проекта.
В файлах main.c и stm32...xx_it.c включить следующий файл:
/* USER CODE BEGIN Includes */
#include <servocontroller.h>
/* USER CODE END Includes */Для работы одного мотора нужно инициализировать (спойлеры разворачиваются):
1. Таймер в режиме энкодера, к каналам которого необходимо подключить энкодер.
После этого в функцию int main() файла main.c нужно добавить следующее (пример для таймера TIM1):
/* USER CODE BEGIN 2 */
__HAL_TIM_CLEAR_IT(&htim1, TIM_IT_UPDATE);
HAL_TIM_Encoder_Start(&htim1, TIM_CHANNEL_ALL);
/* USER CODE END 2 */Убедитесь, что вызываете эти методы перед бесконечным циклом while(1) - в указанном выше плейсхолдере для пользовательского кода
2. Канал другого таймера в режиме генерации ШИМ, к пину которого подключаем вывод PWM драйвера.
Частоту работы ШИМ необходимо выбирать исходя из характеристик драйвера. Например, если в характеристиках драйвера указано до 20 кГц, стоит установить 18 кГц (небольшой запас прочности). Частота от 18 кГц наиболее оптимальна, т.к. это за пределами порога слышимости большинства людей.
После этого в функции int main() файла main.c нужно включить ШИМ на используемых каналах другого таймера (пример для TIM3):
/* USER CODE BEGIN 2 */
HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1);
HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_2);
/* USER CODE END 2 */Убедитесь, что вызываете эти методы перед бесконечным циклом while(1) - в указанном выше плейсхолдере для пользовательского кода
- Два GPIO в режиме output для управления направлением вращения мотора с помощью драйвера.
Для каждого контролируемого привода необходимо объявить экземпляр типа servocontrol_t в файлах main.c:
/* USER CODE BEGIN PV */
servocontrol_t servo1;
/* USER CODE END PV */и stm32...xx_it.c:
/* USER CODE BEGIN EV */
extern servocontrol_t servo1;
/* USER CODE END EV */Теперь необходимо проинициализировать отдельные компоненты модуля необходимыми настройками.
Внимание: servocontrol_t *servo во всех функциях - указатель на экземпляр структуры, т.е. первым аргументом вы подаете: &yourServoName. Далее это объясняться не будет.
Спойлер: инициализация компонентов
void servo_baseInit(servocontrol_t *servo, enum loops servoLoops, float motorSpeed, float gearRatio,
uint8_t reverse);
// servoLoops - количество используемых контуров управления
// Single - регулирование по углу положения вала
// Double - подчиненное регулирование по положению и угловой скорости
// Triple - подчиненное регулирование по положению, угловой скорости и току (пропорционален моменту)
// motorSpeed - скорость привода до редуктора в РАД/С
// gearRatio - передаточное число редуктора. Например, если передаточное число 1:21.3, передайте 21.3.
// Если редуктора нет, или хотите регулировать до привод без учета редукции
// (бывает полезно при большом влиянии вязкого трения редуктора на работу привода), передайте 1.
// reverse - определяет направление вращения, передайте 0 или 1
void servo_encoderInit(servocontrol_t *servo, TIM_HandleTypeDef *htim, uint16_t CPR);
// htim - указатель на обработчик таймера, например &htim1, если используется TIM1
// CPR - количество счетов таймера за один оборот мотора (если использованы два канала, CPR=(PPR*4)-1.
// PPR можно узнать из характеристик энкодера.
void servo_driverInit(servocontrol_t *servo, TIM_HandleTypeDef *htim, uint8_t timerChannel,
GPIO_TypeDef *dir1_Port, uint32_t dir1_Pin, GPIO_TypeDef *dir2_Port, uint32_t dir2_Pin,
uint16_t minDuty, uint16_t maxDuty);
// htim - обработчик таймера, генерирующего ШИМ-сигнал.
// timerChannel - номер канала таймера, который контролирует скорость данного привода (числом: 1/2/3/4)
// Далее пины, управляющие направлением вращения привода через драйвер (с указанием портов)
// minDuty - минимальное значение шим, отличное от нуля, которое будет выдавать микроконтроллер (обычно 0)
// maxDuty - максимальное значение шим, которое будет выдавать микроконтроллер.
// Рекомендую взять значение, равное ARR-1, где ARR - arr регистр таймера
// Стоит уменьшить его на единицу, так как при полном заполнении есть риск перегрева мосфетов.Спойлер: инициализация контуров управления
//------------------------ Следующие инициализаторы - настройки контуров управления ------------------------
//------------ Рекомендуется инициализировать только те контуры, которые будут использоваться --------------
// kp, ki, kd - коэффициенты ПИД регулятора контура
// dt - период работы каждого контура в секундах (очень важно соблюдать эту величину)
// kt - коэффициент алгоритма anti-windup. При отсутствии интегральной составляющей оставить 0
void servo_positionInit(servocontrol_t *servo, float kp, float ki, float kd, float dt, float kt);
void servo_velocityInit(servocontrol_t *servo, float kp, float ki, float kd, float dt, float kt);
void servo_currentInit(servocontrol_t *servo, float ratedCurrent, float kp, float ki, float kd, float dt,
float kt);
// ratedCurrent - номинальный ток мотора в амперахДля опроса данных с энкодера и расчёта необходимых для регулирования величин, в модуле предусмотрено 3 метода:
void servo_positionLoop(servocontrol_t *servo); // Контур положения
void servo_velocityLoop(servocontrol_t *servo); // Контур угловой скорости
void servo_currentLoop(servocontrol_t *servo, float currentFeedback); // Контур тока.
// currentFeedback - текущий ток (А)Используйте только те контуры, которые вы указали в servo_baseInit.
Каждый из вышеперечисленных методов должен вызываться с определенной частотой. Ранее в методах servo_positionInit, servo_velocityInit и servo_currentInit вы указали период интегрирования dt для каждого контура. Величина, равная 1/dt - и есть частота, с которой должен вызываться метод расчета соответствующего контура. Т.е. если для контура положения был указан dt = 0.01, то метод servo_positionLoop должен вызываться с частотой 100 Гц.
Совет 1: Выбор частот
- Ток: функция для расчёта регулятора тока в идеале должна вычисляться с частотой соответствующей обновлению задания для ШИМ силовых ключей. Чтобы для каждой новой коммутации ШИМ уже было рассчитано обновлённое значение на выходе регулятора тока. Но допускается кратно снижать частоту в несколько раз. Чем больше частота расчета контура тока - тем лучше, но необходимо учитывать вычислительные возможности процессора, АЦП и другие факторы. Обычно частоты для вызова регулятора тока - несколько килогерц.
- Угловая скорость: - для контура скорости обычно достаточно частоты 50-200 Гц, но опять же, частота должна быть кратно меньше частоты обновления контура тока. При отстутствии контура тока, частота должна быть кратно меньше частоты ШИМ. Примечание: чем больше разрешение вашего энкодера, тем больше вы можете устанавливать частоту расчёта скорости.
- Положение - контур положения стоит обновлять с той же частотой, что и контур скорости (при его наличии). При остутствии контура скорости, необходимо рассчитать частоту исходя из максимальной скорости вращения вала привода.
Совет 2: Возможная реализация
Самый простой способ обеспечить кратность частот ШИМ и контуров - использовать прерывания по переполнению счетчика таймера, генерирующего ШИМ и программный счётчик. Данный метод не самый "элегантный", если есть достаточное число таймеров для всех контуров, то лучше настроить прерывания с их помощью (при этом наибольший приоритет прерываний должен быть у контура тока, а наименьший у контура положения). Но в случае нехватки таймеров, данный метод тоже сработает. Сначала включаем прерывания на таймере, который генерирует ШИМ-сигнал:
В main.c в функции int main() не забываем запустить прерывания:
/* USER CODE BEGIN 2 */
HAL_TIM_Base_Start_IT(&htim3);
/* USER CODE END 2 */Теперь в файле stm32..xx_it.c создаем целочисленную переменную-счетчик (для примера возьмем int counter) и находим обработчик прерываний (в случае TIM3 это void TIM3_IRQHandler(void)).
В обработчик пишем конструкцию вида:
/* USER CODE BEGIN TIM3_IRQn 0 */
counter++;
if (counter >= 180) {
servo_velocityLoop(&servo1);
counter = 0;
}
/* USER CODE END TIM3_IRQn 0 */Таким образом исходная частота была поделена на 180
На данный момент предусмотрено два вида регулирования - положения и скорости. Для этого существуют соответствующие методы:
void servo_controlPosition(servocontrol_t *servo, float setpoint); // setpoint - угол в радианах
void servo_controlVelocity(servocontrol_t *servo, float setpoint); // setpoint - скорость в рад/сВызывая данные методы, вы можете устанавливать желаемое положение или скорость вала привода (ради чего данный модуль и был создан).
void servo_setPositionTolerance(servocontrol_t *servo, float tolerance);
// Устанавливает допустимую погрешность положения вала в радианах (+-tolerance)
int servo_getState(servocontrol_t *servo);
// Возвращает 0, если нет ошибки по положению, и 1, если ошибка не нулевая
int servo_getDirection(servocontrol_t *servo);
// Возвращает текущее направление вращения вала привода в формате -1/0/1 (0 - нет движения)
// Данные о направлении берутся с энкодераВ папке Servo_Controller_f103 находится пример использования модуля для управления двумя приводами jga25-371 (PPR = 12?, RPM=11500, Gear ratio = 1:21.3). На данный момент пример без контура тока (still in progress). Пример написан для микроконтроллера stm32f103c8t6 (отладочная плата bluepill)