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

Сервопривод представляет собой электропривод с обратной связью, который поддерживает заданные параметры на исполнительном органе. Конструктивно состоит из электродвигателя, редуктора и блока управления. На выходном валу редуктора установлен датчик положения, с помощью которого блок управления отслеживает положения выходного вала, обеспечивая обратную связь. Я буду рассматривать китайский сервопривод под названием 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
Прошивка МК и исходник (автоматический возврат вала сервопривода в среднее положение)