Управление сервоприводом на микроконтроллере



Управление сервоприводом на МК
Сервопривод представляет собой электропривод с обратной связью, который поддерживает заданные параметры на исполнительном органе. Конструктивно состоит из электродвигателя, редуктора и блока управления. На выходном валу редуктора установлен датчик положения, с помощью которого блок управления отслеживает положения выходного вала, обеспечивая обратную связь. Я буду рассматривать китайский сервопривод под названием Tower Pro SG90, диапазон угла поворота вала составляет 180 градусов, скорость поворота 60 градусов/0,3 секунды, применяется в основном в радиоуправляемых игрушках, роботах и т.д. Сервопривод имеет три вывода для подключения, два вывода красный (+) и коричневый (-) для питания, оранжевый вывод для управления, напряжение питания составляет 5В.

Сервоприводы я заказывал здесь. Управляется сервопривод повторяющимися импульсами определенной длительности. Ниже представлена картинка с управляющими импульсами для сервопривода SG90:
Диаграмма управляющих импульсов сервопривода
При подаче импульсов длительностью в 1,5 мс, вал сервопривода установится в среднее положение. Для импульсов шириной в 0,6 мс, вал повернется на 90 градусов по часовой стрелке относительно среднего положения (если смотреть на вал сервопривода сверху). Если ширина импульсов составит 2,5 мс, вал повернется на 90 градусов против часовой стрелки относительно среднего положения. Таким образом, положение вала задается шириной управляющего импульса. Период следования импульсов может составлять от 10 до 20 мс, за стандарт принято значение 20 мс (частота 50Гц). На самом деле данный сервопривод (SG90) может повернуться еще на несколько градусов от крайних положений, то есть диапазон угла поворота чуть больше 180 градусов. Фактически угол поворота ограничивается механическими упорами на шестеренке редуктора, вышеприведенные временные интервалы управляющих импульсов ограничивают диапазон угла поворота в пределах 180 градусов, не позволяя поворачиваться валу до механических упоров.

Ниже представлена схема подключения сервопривода к микроконтроллеру PIC16F628A:
Управление сервоприводом, схема
Управление сервоприводом осуществляется двумя кнопками, при нажатии кнопки SB1 вал сервопривода плавно поворачивается по часовой стрелке, то есть происходит уменьшение ширины управляющих импульсов на линии RB4, кнопка SB2 соответственно поворачивает вал против часовой стрелки, ширина импульсов увеличивается.
Ниже представлен код программы для микроконтроллера:

Микроконтроллер работает от внутреннего тактового генератора на частоте 4 МГц. Длительность временных интервалов для управляющих импульсов отсчитываются внутренним 16-ти разрядным таймером TMR1. При частоте в 4 МГц и коэффициенте предделителя таймера 1:1, переполнение наступит через 65536 мкс. Используя прерывание по переполнению таймера, можно отсчитывать различные временные интервалы, в диапазоне 1-65536 мкс.

Управляющие импульсы для сервопривода генерируются в подпрограмме обработки прерываний. В начале основной программы в регистры счета (t1H, t1L) записывается число 1500, соответствующее временной задержке в 1500 мкс (среднее положение вала сервопривода), следом в промежуточные регистры (t1HH, t1LL) записывается инвертированное значение числа 1500. Регистры счета используются для хранения текущей длительности импульса, промежуточные регистры выступают в роли буфера, из которых инвертированное значение длительности копируется в регистры таймера TMR1 (TMR1H, TMR1L). В регистрах счета, значение длительности представлено в удобной форме для математических операций сложения и вычитания, но перед записью в регистры таймера TMR1, значение длительности необходимо инвертировать, так как прерывание генерируются по переполнению таймера, а не по достижении конкретного значения.

Далее настраивается таймер TMR1, разрешается прерывание периферийных модулей, сбрасывается флаг переполнения, разрешается прерывание по переполнению таймера, запуск таймера, ну и разрешение глобальных прерываний.

По переполнению таймера происходит переход в обработчик прерываний, где в первую очередь выполняется остановка таймера и опрос флага очередности flag,0 (в начале основной программы флаг сбрасывается). Если флаг сброшен, происходит переход на метку t1, далее происходит перезапись длительности управляющего импульса (1500 мкс) из промежуточных регистров (t1HH, t1LL) в регистры таймера TMR1, установка флага очередности, установка в 1 линии RB4 (название линии serv). После этого сбрасывается флаг прерывания, запуск таймера и выход из обработчика прерываний. Через 1,5 мс произойдет очередной переход в обработчик прерываний по переполнению таймера, на этот раз флаг очередности равен 1, и произойдет переход на метку t2. Далее происходит сброс флага очередности и линии RB4 в 0, затем в регистры таймера записывается значение длительности паузы между импульсами, равное 18,5 мс. Дополнительно проверяется значение регистра tmp_off , который выступает в роли счетчика времени для отключения таймера TMR1, о нем будет сказано позже. Если значение регистра tmp_off не равно 24, происходит инкремент регистра, далее сброс флага прерывания, запуск таймера и выход из подпрограммы обработки прерываний.

По истечении 18,5 мс произойдет прерывание и переход в обработчик прерываний, и цикл повторится заново. Таким образом, один период управляющего сигнала (цикл) генерируется за 2 прерывания, причем длительность периода будет меняться в пределах 19,1-21 мс, в зависимости от длительности управляющего импульса.

В основной программе идет опрос кнопок SB1 и SB2, при нажатии которых происходит вызов подпрограмм povorot_1 и povorot_2 соответственно. В подпрограмме povorot_1 происходит уменьшение длительности управляющего импульса на 10 мкс (вычитание числа 10 из регистров счета t1H, t1L), в подпрограмме povorot_2 увеличение на 10 мкс (прибавление числа 10 к регистрам счета t1H, t1L). Если значение длительности меньше 0,6 мс или больше 2,5 мс выполняется выход из подпрограмм без изменения значений регистров счета, то есть происходит ограничение длительности управляющего импульса. После изменения длительности импульса происходит вызов подпрограммы perezap, в которой значения регистров счета перезаписывается в промежуточные регистры (t1HH, t1LL) в инвертированном виде. Во время перезаписи запрещается прерывание по переполнению таймера TMR1, это необходимо для получения корректного значения длительности управляющего импульса в регистрах таймера. Если не запретить прерывания, возможен случай, когда прерывание произойдет после записи старшего промежуточного регистра t1HH, а значение младшего регистра t1LL при этом останется без изменений. В таком случае в обработчике прерываний, в регистры таймера TMR1 запишется некорректное значение длительности управляющего импульса из промежуточных регистров (t1HH, t1LL). После возврата из подпрограмм perezap и povorot_1, povorot_2, вызывается подпрограмма паузы pausknp (10,5 мс), после возврата продолжается опрос кнопок.

После экспериментов с сервоприводом я заметил одну особенность, когда на вал сервопривода действует небольшой по величине постоянный внешний момент силы (нахождение под нагрузкой), происходит микро-поворот вала, сервопривод при этом пытается поддерживать заданное положение, в результате чего постоянно “жужжит”, потребляя энергию. Без нагрузки такого поведения не возникает. Чтобы избавится от этого явления, я реализовал в программе счетчик времени, если в течение определенного времени длительность управляющего импульса не меняется, то генерирование импульсов на линии RB4 прекращается. Импульсы возобновляются при нажатии кнопок SB1, SB2.

В качестве счетчика времени используется регистр tmp_off , о котором я упоминал выше, значение регистра инкрементируется в обработчике прерывания, во втором прерывании, когда выполняется запись длительности паузы между импульсами (18,5 мс) в таймер TMR1. Таким образом, счетчик инкрементируется один раз за период управляющего сигнала. Там же в обработчике прерываний происходит проверка значения счетчика, если оно равно числу 24, происходит выход из обработчика прерываний без запуска таймера TMR1, в результате генерирование управляющих импульсов прекращается. При среднем значении периода в 20 мс, генерация импульсов прекратиться через (20мс)х24=480 мс.

Генерация импульсов возобновляется при нажатии кнопок SB1, SB2, в подпрограмме perezap выполняется запуск таймера TMR1, там же происходит очистка регистра tmp_off. При длительном нажатии кнопок, подпрограмма perezap вызывается многократно и регистр tmp_off постоянно обнуляется, то есть прекращение подачи управляющих импульсов не происходит.

Величина, на которую изменяется длительность управляющего импульса при вызове подпрограмм povorot_1, povorot_2 составляет 10 мкс, всего получается (2500мкс-600мкс)/10мкс=190 дискретных значений длительности управляющего импульса. Дискретность угла поворота составит 180/190=0,94 градуса. Пауза после каждого дискретного изменения равна 10,5 мс, соответственно поворот вала сервопривода на 180 градусов займет (10,5мс)х190=1,995 секунды.

На основе этих сервоприводов я разработал поворотную платформу с радиоуправлением.

Макетная плата с сервоприводом
Ниже представлен видеоролик демонстрирующий работу сервопривода:

Прошивка МК и исходник + модель Proteus 7.10

Последние записи:

Комментариев 6 на “Управление сервоприводом на микроконтроллере

  1. Ок, пишу там где надо. У PIC16F628A есть ССР (аппаратный ШИМ) и есть внутренний генератор, который можно запустить на частоте 32 кГц. При таких условиях можно с легкостью управлять сервоприводом SG90. При этом надо лишь записать число, определяющее период (в нашем случае 20мс) в PR2 и изменяя значения в CCPR1L двигать вал привода. Вроде се просто, но я как ни бился, не могу подобрать эти значения для PR2, CCPR1L, и предделителя…
    Может чем поможете….

    • Почитайте мою статью про ШИМ и АЦП чтобы лучше разобраться с ШИМ http://radiolaba.ru/microcotrollers/ispolzovanie-moduley-atsp-i-shim-v-mikrokontrollerah-pic16.html

      А так формула расчета частоты ШИМ: Fшим=Fosc/(4×(PR2+1)×(коэффициент предделителя TMR2)), отсюда выводим значение PR2=(Fosc/(4×(Fшим)×(коэффициент предделителя TMR2)))-1, для коэффициента предделителя TMR2 равному 1 и частоте тактового генератора 32кГц (частота ШИМ 50Гц) получаем PR2=(32000/(4×50×1))-1=159, при этом разрядность ШИМ составит =log(Fosc/Fшим)/log(2)=log(32000/50)/log(2)=9,32 бит, это 640 дискретных значений для периода в 20мс, длительность одного дискретного значения 31,25мкс=Tosc. Причем значение скважности придется загружать не только в регистр CCPR1L но и в два младших бита регистра CCP1CON(5:4), так как отсчет разрядов начинается именно с них.

      Для сервопривода управляющий сигнал меняется в пределах 600мкс-2,5мс, для этого интервала вы получите всего (2500-600)/31,25мкс=61 дискретных положений вала сервопривода (дискретность угла поворота составит 180/61=2,9 градуса). Для получения длительности высокого уровня сигнала в 600мкс, вам надо загрузить в регистры скважности ШИМ (CCPR1L, CCP1CON(5:4)) число (600/31,25)=19, для 2500мс число (2500/31,25)=80, то есть получаем пределы чисел для скважности ШИМ 19-80, еще раз повторю загружать скважность надо с двух младших регистров CCP1CON(5:4).

    • Думаю что можно, из соображений что два входа можно соединять параллельно. Но я не проверял такое на сервоприводах.

  2. Здравствуйте!
    Можно ли сделать на одном контроллере управление двумя сервоприводами? То есть нужно управлять по 4-м портам,два из которых влево\вправо для первого серво привода, остальные два-для второго. Могли бы Вы мне помочь в этой задаче? Для первого сервопривода угол нужен 180гр.,для второго порядка 60гр.

    • Здравствуйте. Да такое возможно.
      Управление по 4-м портам это вы имеете в виду управление с помощью 4 кнопок? У меня есть подобная статья на сайте, называется — Поворотная платформа с дистанционным управлением (ссылка есть в конце статьи), там 2 сервопривода управляются 4-мя кнопками, можно регулировать скорость поворота. Возможно это то что вам нужно, только надо вырезать оттуда радиоуправление.

Добавить комментарий

Ваш e-mail не будет опубликован. Обязательные поля помечены *