![[personal profile]](https://www.dreamwidth.org/img/silk/identity/user.png){kind=small}
eddy_em(дал заголовок на английском, т.к. сколько ни гуглил по этой теме, ничего толкового не попадалось).
Возможно (скорей всего), какие-то неровности еще остались, но в основном управление уже работает. Функционал с кнопками и концевиками уже в железе буду отрабатывать. Часть с CAN вообще не тестировал еще + не брался за I2C, ну и надо бы одновременно три шаговика погонять: интересно (правда, жалко разъемы тратить, ну да ладно, их есть у меня).
Базовые настройки хранятся во флеш-памяти, но для сохранения новых нужно дать специальную команду, поэтому экспериментировать можно, не боясь что-то поломать. Так как часть параметров вычисляется с инициализацией таймеров двигателей и энкодеров, есть команда "переинициализации".
У шаговых двигателей есть несколько состояний:
Я, кстати, забыл добавить состояние 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 раза рубить скорость при проблемах, но мне откровенно лень (да еще и пришлось бы автонастройку параметров регулятора для каждой оси выдумывать).
Возможно (скорей всего), какие-то неровности еще остались, но в основном управление уже работает. Функционал с кнопками и концевиками уже в железе буду отрабатывать. Часть с 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 раза рубить скорость при проблемах, но мне откровенно лень (да еще и пришлось бы автонастройку параметров регулятора для каждой оси выдумывать).
