Емельянов Эдуард Владимирович (eddy_em) wrote,
Емельянов Эдуард Владимирович
eddy_em

Category:

Stepper motor closed loop control algorithm

(дал заголовок на английском, т.к. сколько ни гуглил по этой теме, ничего толкового не попадалось).
Возможно (скорей всего), какие-то неровности еще остались, но в основном управление уже работает. Функционал с кнопками и концевиками уже в железе буду отрабатывать. Часть с CAN вообще не тестировал еще + не брался за I2C, ну и надо бы одновременно три шаговика погонять: интересно (правда, жалко разъемы тратить, ну да ладно, их есть у меня).

Базовые настройки хранятся во флеш-памяти, но для сохранения новых нужно дать специальную команду, поэтому экспериментировать можно, не боясь что-то поломать. Так как часть параметров вычисляется с инициализацией таймеров двигателей и энкодеров, есть команда "переинициализации".
У шаговых двигателей есть несколько состояний:
RELAX
ничего не делаем — стоим на месте,

ACCEL
фаза ускорения от минимальной до максимальной скорости,

MOVE
движение с максимальной скоростью,

MVSLOW
движение с минимальной скоростью (в случае проблем на стадии замедления),

DECEL
фаза замедления до минимальной скорости и останова,

STALL
аналогична RELAX (и снимается при первом же движении), чтобы понять, что двигатель во что-то уперся,

ERR
несуществующее состояние.


Я, кстати, забыл добавить состояние STALL в КА, надо это сделать.
У таймеров двигателей TIM->PSC=2 (16МГц), режим ШИМ1, ARR может изменяться от 99 (5000 шагов в секунду на микрошаге 1:32) до 65535 (7 шагов/с). Значение CCR1 на 3 меньше ARRMIN (т.е. на предельной скорости длина нулевого импульса будет меньше 0.2мкс, что может вызвать проблемы).
В настройках каждого мотора указываются предельные скорости, ускорения, микрошаги, максимальное перемещение от нуля в любую сторону, количество импульсов энкодера на оборот, допустимые отклонения в импульсах энкодера на шаг двигателя (±3 оказалось нормальным), реакция на концевики (игнор, стоп при движении в любом направлении - для съезда с концевика в этом состоянии надо кратковременно сменить на игнор -, стоп при движении в минус). Также указываются флаги: реверс вращения мотора, реверс вращения энкодера, наличие энкодера, снятие напряжения с двигателя после останова, инверсия состояния концевиков, "эмуляция сервопривода" (контроль положения вала в состоянии останова).
При запуске движения в определенное положение сначала проверяем допустимость этого положения, далее инициализируем переменные: целевое положение, предыдущие значения счетчика шагов и энкодера и текущую скорость (для начала - минимальная). Вычисляем параметры ускорения/замедления (позицию, с которой нужно будет начинать замедляться, начальную скорость ускорения и начальное время). Пересчитываем значение TIM->ARR, устанавливаем состояние в ACCEL и запускаем таймер.
Процесс счета шагов ведется в прерываниях таймеров по CC, т.е. еще до окончания счетного импульса (чтобы успеть тормознуть без лишнего микрошага). Вначале проверяем концевик: если активен, устанавливаем флаг останова (тормозим всегда исключительно по завершении целого шага). Если насчитали нужное количество микрошагов, инкрементируем счетчик шага и проверяем флаги останова по требованию и останова на позиции. Если нужно остановиться, то сохраняем текущее положение как положение цели (для случая, если включен режим удержания положения), выключаем таймер и при необходимости снимаем напряжение с мотора.
Если у двигателя есть энкодер, то при движении из автомата проверки состояния двигателя сначала вызывается проверка заклинивания. Сравнивается смещение относительно предыдущего отсчета энкодера и шагов двигателя. В случае несоответствия в пределах, заданных в настройках, инкрементируется счетчик, и если он не превышает заданной величины (5, например), то текущая скорость уменьшается в 2 раза и производится перевычисление параметров ускорения (т.е. двигатель опять переходит в состояние ACCEL, в каком бы состоянии он ни был, но соответствующим образом и начало DECEL пересчитывается). Если же счетчик велик, то выставляется флаг останова (и здесь же нужно было бы в STALL отправить состояние КА двигателя).
Конечный автомат проверки состояния шаговиков вызывается не чаще, чем раз в 10мс. Последовательно проверяются состояния двигателей.
В состоянии RELAX, если у мотора есть энкодер и указан флаг удержания позиции, то по энкодеру пересчитывается текущее (в шагах) положение двигателя плюс проверяется, не смылся ли он с заданной позиции. Если смылся, вызывается команда движения в заданную позицию.
Как я уже говорил, во всех состояниях движения сначала проверяется, не застрял ли вал. Если застрял, никаких действий не производится.
В состоянии ACCEL вычисляется новая скорость: V=V₀+A(T-T₀). Если V≥V_max, то переходим с V_max в состояние MOVE, иначе просто пересчитываем регистр ARR на новую скорость. Далее на случай движения по треугольному рампу (если расстояние небольшое и достичь максимальной скорости не получится) проверяется, не пора ли тормозить (т.е. достигнута позиция начала торможения). Если пора, переходим в состояние DECEL.
В состоянии MOVE только проверяем, не пора ли замедляться.
В состоянии DECEL замедляемся: V=V₀-A(T-T₀). Если достигли минимально возможной скорости, переходим с нею в состояние MVSLOW, где по-хорошему, надо бы тоже STALL проверять (а я забыл - вот, как раз сейчас и добавил).
Вот такой несложный код на четыре сотни строк и обеспечивает движение мотора с обратной связью. Можно, конечно, ПИД-регулятор добавить вместо того, чтобы грубо в 2 раза рубить скорость при проблемах, но мне откровенно лень (да еще и пришлось бы автонастройку параметров регулятора для каждой оси выдумывать).
Reposted from dreamwidth: https://eddy-em.dreamwidth.org/310517.html.
Tags: steppers, stm32, железяки
Subscribe

  • Протокол контроллера TeA

    Постепенно развиваю "чайный" контроллер. Думаю, по протоколу уже вряд ли что-то нужно будет добавить. Намедни отладил CAN (т.к. я им с самого начала…

  • Акуна, мать тата…

    Еще на первой сборке в среду появились какие-то непонятные проблемы: то здесь, то там не было контакта, в итоге только один шаговик нормально…

  • TeA обрастает проводами

    Как говорится, глаза боятся, а руки делают. С понедельника занимаюсь завершением сборки и прокладкой кабелей. Пришлось еще немного "по месту"…

promo eddy_em september 3, 12:13 8
Buy for 10 tokens
Уже больше полугода занимаюсь разработкой, вот, наконец-то в мастерских взялись за меня и начали выдавать первые детали. Сегодня сделал тестовую сборку (как обычно, местами пришлось "доработать напильником"): Пока прибор без названия (да и как-то не лезет в голову ничего, у меня нет…
  • Post a new comment

    Error

    Anonymous comments are disabled in this journal

    default userpic
  • 1 comment