Подключаем сервопривод к Raspberry Pi

Сервопривод - механизм с электрическим мотором, который может поворачивать вал на заданный угол и сохранять это положение

Подключение

Сервоприводы подключаются тремя проводами. Два провода (чёрный и красный) - питание, третий (оранжевый, жёлтый, белый или синий) - сигнальный.

Для моего сервопривода MG 996R провода имеют такой цвет. Запитал я его от Raspberry Pi, подключив Vcc к пину №4 (5v), Ground к пину #6 (GND). Оранжевый сигнальный провод - к пину №12 (PWM)

Принцип работы

По сигнальному проводу на сервопривод подаётся ШИМ сигнал. Для любительских сервоприводов частота сигнала обычно равна пятидесяти герцам (импульсы через каждые 20мс). От длительности импульса (а не от скажности импульсов) зависит, на какой угол электромотор повернёт выходной вал сервопривода.

Плата сервопривода преобразует длительность импульса в управляющее напряжении (Uупр), которое поступает на сумматор. Также на сумматор приходит напряжение обратной связи (Uobr), которое регулируется потенциометром, соединённым с выходным валом сервопривода. Сумматор вычисляет разницу между Uупр и Uobr, разница напряжений усиливается и подаётся на электромотор. Электромотор вращает выходной вал, вместе с выходной валом вращается и меняет своё сопротивление потенциометр, из-за изменения сопротивления меняется напряжение обратной связи. Электромотор остановится, когда разница между напряжением обратной связи и управляющим напряжением станет равна нулю.

Программирование

Теперь попробуем всё это запрограммировать. Для работы с gpio я предпочитаю использовать библиотку wiringPi. В Raspberry Pi аппаратный шим находится на пинах BCM18 (порядковый номер 12, в нотификации wiringPi - 1).

Тактирующий осциллятор на аппаратном ШИМ Raspberry Pi имеет частоту 19.200.000 Гц (19.2 МГц). Чтобы получить частоту в 50Гц, необходимую для сервопривода, зададим делителю частоты значение 192 при помощи функции pwmSetClock , а значение соответствующее максимальному коэффициенту заполнения импульса равным 2000 функцией pwmSetRange .

19.200.000 / 192 / 2000 = 50

Как было сказано выше - угол поворота вала зависит от длительности импульса. Длительность импульса зависит от величины коэффициента заполнения, который задаётся вторым аргументом функции pwmWrite . При коэф. заполнения 1 (значение 2000) длительность импульса составит 20мс, при коэф. заполнения 0.05 (значение 100) 1мс

Большинство сервоприводов рассчитаны на приём импульсов длительностью от 1 до 2 мс

1мс - крайнее левое положение, 1.5мс - центральное положеник, 2мс - крайнее правое положение. Задавая длительность импульса в диапазоне 1-2мс, можно повернуть вал сервопривода в нужное положение

В реальности диапазон длительности импульсов, приводящих к повороту вала, может несколько отличаться и устанавливается экспериментальным путём. Для моего сервопривода (MG 996R) он составил 0.35мс (коэф. заполнения 1.75%) - 2.63мс (коэф. заполнения 13.15%)

Рассчёт угла поворота вала сервопривода

Напишем функцию, которая переводит требуемый угол поворота в градусах в значение коэффициента заполнения, который выступает аргументом функции pwmWrite и определяет на какой угол повернётся вал сервопривода.

int getDCValue(int angle, int maxPWMRange)
{
   //DC - Duty Cycle, коэффициент заполнения
   float minDC = 1.75;
   float maxDC = 13.15;
   int requiredDC = (angle/180*(maxDC-minDC)+minDC)/100*maxPWMRange; 
   return requiredDC;
}

Полностью программа будет выглядеть так. Угол поворота задаётся как параметр в командной строке при запуске программы:

//servo.c
#include <stdio.h>
#include <stdlib.h>
#include  <wiringPi.h> 

int getDCValue(int angle, int maxPWMRange)
{
   float minDC = 1.75;
   float maxDC = 13.15;
   int requiredDC = (angle*(maxDC-minDC)/180+minDC)/100*maxPWMRange; 
   return requiredDC;
}

void main(int argc, char *argv[])
{
 if (argc==2)
 {
  int maxPWMRange = 2000;
  int rotateAngle = atoi(argv[1]);
  int dutyCycle = getDCValue(rotateAngle,maxPWMRange);
  wiringPiSetup ();
  pinMode (1, PWM_OUTPUT); //переводим вывод BCM18 в режим ШИМ выхода
  pwmSetMode(PWM_MODE_MS);   
  pwmSetClock(192);
  pwmSetRange(maxPWMRange);
  pwmWrite (1, dutyCycle);
  printf ("set PWM  value %d \n", dutyCycle);
 }
 else
 {
  printf ("Vvedite ugol povorota");
 }
}

Компилируем

gcc ./servo.c -o servo -lwiringPi

Запускаем:

pi@raspberrypi:~ $ sudo ./servo 60

Вал сервопривода встаёт в положение 60 градусов