Управление сервоприводом SG90 с помощью PCA9685 из Python на Raspberry Pi/Orange Pi/Banana Pi

Сервопривод — это мотор-редуктор, способный поворачивать выходной вал в заданное положение (на заданный угол) и удерживать его в этом положении, вопреки сопротивлениям и возмущениям. Сервопривод Tower Pro 9g SG90 не имеет мощные характеристики (всего 1,2-1,6 кг*см), но имеет недорогую цену. Отлично подходит для управления небольшими легкими механизмами под управлением контроллеров Arduino, Raspberry Pi  и т.п.. Рабочее напряжение Tower Pro 9g SG90 от 3В до 7.2В, угол поворота ограничен диапазоном от 0 до 180 градусов (в реальность — чуть больше).

В этой статье будем управлять сервоприводом SG90 с помощью PCA9685 из Python на Orange Pi PC. Можно использовать Raspberry Pi, Banana Pi, NanoPi или любой другой мини-компьютер под управлением ОС Linux имеющий I2C порт.

Библиотека на Python для PCA9685

Поскольку библиотека Adafruit_Python_PCA9685 для работы с PCA9685 из Python работает только на Raspberry Pi, она была переписана так, чтобы ее можно было использовать на Orange Pi и Banana Pi. Теперь используется SMBus в качестве I2C драйвера, как установить тут: SMBus: Работа с шиной I2C на Python в Raspberry Pi/Orange Pi/Banana Pi.

Файл PCA9685.py

import logging
import time
import math

# Based on Adafruit Lib:
# https://github.com/adafruit/Adafruit_Python_PCA9685/blob/master/Adafruit_PCA9685/PCA9685.py

# Default address:
PCA9685_ADDRESS    = 0x40

# Registers/etc:
MODE1              = 0x00
MODE2              = 0x01
SUBADR1            = 0x02
SUBADR2            = 0x03
SUBADR3            = 0x04
PRESCALE           = 0xFE
LED0_ON_L          = 0x06
LED0_ON_H          = 0x07
LED0_OFF_L         = 0x08
LED0_OFF_H         = 0x09
ALL_LED_ON_L       = 0xFA
ALL_LED_ON_H       = 0xFB
ALL_LED_OFF_L      = 0xFC
ALL_LED_OFF_H      = 0xFD

# Bits:
RESTART            = 0x80
SLEEP              = 0x10
ALLCALL            = 0x01
INVRT              = 0x10
OUTDRV             = 0x04

# Channels
CHANNEL00          = 0x00
CHANNEL01          = 0x01
CHANNEL02          = 0x02
CHANNEL03          = 0x03
CHANNEL04          = 0x04
CHANNEL05          = 0x05
CHANNEL06          = 0x06
CHANNEL07          = 0x07
CHANNEL08          = 0x08
CHANNEL09          = 0x09
CHANNEL10          = 0x0A
CHANNEL11          = 0x0B
CHANNEL12          = 0x0C
CHANNEL13          = 0x0D
CHANNEL14          = 0x0E
CHANNEL15          = 0x0F

class PCA9685(object):
    def __init__(self, i2cBus, address=PCA9685_ADDRESS):
        self.i2cBus = i2cBus
        self.address = address
        self.begin()

    def begin(self):
        """Initialize device"""
        self.set_all_pwm(0, 0)
        self.i2cBus.write_byte_data(self.address, MODE2, OUTDRV)
        self.i2cBus.write_byte_data(self.address, MODE1, ALLCALL)
        time.sleep(0.005)                                         # wait for oscillator
        mode1 = self.i2cBus.read_byte_data(self.address, MODE1)
        mode1 = mode1 & ~SLEEP                                    # wake up (reset sleep)
        self.i2cBus.write_byte_data(self.address, MODE1, mode1)
        time.sleep(0.005)                                         # wait for oscillator

    def reset(self):
        self.i2cBus.write_byte_data(self.address, MODE1, RESTART)
        time.sleep(0.01)

    def set_address(self, address):
        """Sets device address."""
        self.address = address

    def set_i2c_bus(self, i2cBus):
        """Sets I2C Bus."""
        self.i2cBus = i2cBus

    def set_pwm(self, channel, on, off):
        """Sets a single PWM channel."""
        self.i2cBus.write_byte_data(self.address, LED0_ON_L + 4 * channel, on & 0xFF)
        self.i2cBus.write_byte_data(self.address, LED0_ON_H + 4 * channel, on >> 8)
        self.i2cBus.write_byte_data(self.address, LED0_OFF_L + 4 * channel, off & 0xFF)
        self.i2cBus.write_byte_data(self.address, LED0_OFF_H + 4 * channel, off >> 8)

    def set_all_pwm(self, on, off):
        """Sets all PWM channels."""
        self.i2cBus.write_byte_data(self.address, ALL_LED_ON_L, on & 0xFF)
        self.i2cBus.write_byte_data(self.address, ALL_LED_ON_H, on >> 8)
        self.i2cBus.write_byte_data(self.address, ALL_LED_OFF_L, off & 0xFF)
        self.i2cBus.write_byte_data(self.address, ALL_LED_OFF_H, off >> 8)

    def set_pwm_freq(self, freq_hz):
        """Set the PWM frequency to the provided value in hertz."""
        prescaleval = 25000000.0                                  # 25MHz
        prescaleval /= 4096.0                                     # 12-bit
        prescaleval /= float(freq_hz)
        prescaleval -= 1.0
        prescale = int(math.floor(prescaleval + 0.5))
        oldmode = self.i2cBus.read_byte_data(self.address, MODE1)
        newmode = (oldmode & 0x7F) | 0x10                         # sleep
        self.i2cBus.write_byte_data(self.address, MODE1, newmode) # go to sleep
        self.i2cBus.write_byte_data(self.address, PRESCALE, prescale)
        self.i2cBus.write_byte_data(self.address, MODE1, oldmode)
        time.sleep(0.005)
        self.i2cBus.write_byte_data(self.address, MODE1, oldmode | 0x80)

    def __enter__(self):
        return self

    def __exit__(self, exception_type, exception_value, traceback):
        self.reset()

Описание методов (функций)

__init__()

Конструктор класса.

__init__(self, i2cBus, address=PCA9685_ADDRESS)

Параметры
i2cBus — Объект типа PCA9685.
address — I2C адрес устройства. По умолчанию PCA9685_ADDRESS = 0x40.

begin()

Инициализация устройства.

begin(self)

set_address()

Установка адреса устройства.

set_address(self, address)

Параметры
address — I2C адрес устройства.

set_i2c_bus()

Установка I2C шины.

set_i2c_bus(self, i2cBus)

Параметры
i2cBus — Объект типа PCA9685.

set_pwm()

Устанавливает ШИМ одного из выводов PCA9685.

set_pwm(self, channel, on, off)

Параметры
channel — Один из выводов PWM от 0 до 15.
on — В какой момент цикла из 4096 частей включить ШИМ.
off — В какой момент цикла из 4096 частей выключить ШИМ.

set_all_pwm()

Устанавливает ШИМ на все выводы PCA9685.

set_all_pwm(self, on, off)

Параметры
on — В какой момент цикла из 4096 частей включить ШИМ.
off — В какой момент цикла из 4096 частей выключить ШИМ.

set_pwm_freq()

Устанавливает частоту ШИМ для всего чипа, до ~ 1,6 кГц.

set_pwm_freq(self, freq_hz)

Параметры
freq_hz — Частота в Герцах.

Библиотека на Python для сервоприводов

Для более удобного управления сервоприводом, основные функции были собраны в одном классе — ServoPCA9685. Тут можно найти минимальную (servo_min = 130) и максимальную (servo_max = 510) длину импульса для безопасного управления сервоприводом SG90.

# Configure min and max servo pulse lengths
servo_min = 130
servo_max = 510

Если ваш сервопривод работает с другими значениями, тогда вы можете редактировать их.

Файл ServoPCA9685.py

import time
# Servo with PCA9685 implementation

# Configure min and max servo pulse lengths
servo_min = 130 # Min pulse length out of 4096 / 150/112
servo_max = 510 # Max pulse length out of 4096 / 600/492

def map(x, in_min, in_max, out_min, out_max):
    return (x - in_min) * (out_max - out_min + 1) / (in_max - in_min + 1) + out_min

class ServoPCA9685(object):
    def __init__(self, pca9685, channel):
        self.pca9685 = pca9685
        self.channel = channel
        self.set_pwm_freq(50)
        self.set_pulse(300)

    def set_pwm_freq(self, freq=50):
        self.pca9685.set_pwm_freq(freq)
        time.sleep(0.005)

    def set_angle(self, angle):
        self.set_pulse(map(angle, 0, 180, servo_min, servo_max))

    def set_pulse(self, pulse):
        if pulse >= servo_min and pulse <= servo_max:
            self.pca9685.set_pwm(self.channel, 0, pulse)
            time.sleep(0.005)

    def disable(self):
        self.pca9685.set_pwm(self.channel, 0, 0)
        time.sleep(0.005)

Описание методов (функций)

__init__()

Конструктор класса.

__init__(self, pca9685, channel)

pca9685 — Объект типа PCA9685.
channel — Один из ШИМ выводов PCA9685 от 0 до 15.

set_pwm_freq()

Установка частоты ШИМ для вашего сервопривода.

set_pwm_freq(self, freq=50)

freq — Частота в Герцах. По умолчанию freq=50.

set_angle()

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

set_angle(self, angle)

angle — Угол от 0 до 180 градусов.

set_pulse()

Установка длины импульса.

set_pulse(self, pulse)

pulse — Длина ШИМ импульса.

disable()

Отключение сервопривода (установка длины импульса в ноль «0»).

disable(self)

Примеры программ

Схема подключения сервопривода SG90 к PCA9685

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

Чтобы управлять сервоприводом посредством PCA9685 нужно соблюдать следующие шаги:

1. Нужно открыть шину I2C «0» (или «1»);

   i2cBus = smbus.SMBus(0)

2. Создаём объект класса PCA9685, а в качестве параметра конструктора используем выше созданный объект: i2cBus;

   pca9685 = PCA9685.PCA9685(i2cBus)

3. Создаём объект класса ServoPCA9685 для управления одного сервопривода, в качестве первого параметра используем выше созданный объект, pca9685, а второй параметр — это номер канала PCA9685, можно выбрать следующие значения: PCA9685.CHANNEL00, PCA9685.CHANNEL01, PCA9685.CHANNEL02, …, PCA9685.CHANNEL15 или номера от 0 до 15;

   servo00 = ServoPCA9685.ServoPCA9685(pca9685, PCA9685.CHANNEL00)

4. Для управления сервоприводом можно использовать два метода, а именно: set_pulse(pulse), где pulse — это длина ШИМ импульса от servo_min = 130 до servo_max = 510; и set_angle(angle), где angle — это угол поворота от 0 до 180 градусов, метод (функция) пропорционально переносит значение из текущего диапазона значений в градусах (от 0 до 180) в новый диапазон (от 130 до 510) в импульсах.

Нижеприведённый пример кода поварачивает сервопривод в одну сторону,

# 130 -> 510
for pulse in range(ServoPCA9685.servo_min, ServoPCA9685.servo_max + 1):
    servo00.set_pulse(pulse)
    time.sleep(0.01)

потом в другую

# 510 -> 130
for pulse in reversed(range(ServoPCA9685.servo_min, ServoPCA9685.servo_max + 1)):
    servo00.set_pulse(pulse)
    time.sleep(0.01)

с использованием метода set_pulse(pulse), а в конце отключает подаваемый на сервопривод ШИМ.

servo00.disable()

Файл servo_1x_pulse.py

Пример управления сервоприводом используя метод set_pulse(pulse).

import time
import smbus
import PCA9685
import ServoPCA9685

i2cBus = smbus.SMBus(0)
pca9685 = PCA9685.PCA9685(i2cBus)
servo00 = ServoPCA9685.ServoPCA9685(pca9685, PCA9685.CHANNEL00)

# 130 -> 510
for pulse in range(ServoPCA9685.servo_min, ServoPCA9685.servo_max + 1):
    servo00.set_pulse(pulse)
    time.sleep(0.01)

# 510 -> 130
for pulse in reversed(range(ServoPCA9685.servo_min, ServoPCA9685.servo_max + 1)):
    servo00.set_pulse(pulse)
    time.sleep(0.01)

servo00.disable()

Файл servo_1x_angle.py

Пример управления сервоприводом используя метод set_angle(angle).

import time
import smbus
import PCA9685
import ServoPCA9685

i2cBus = smbus.SMBus(0)
pca9685 = PCA9685.PCA9685(i2cBus)
servo00 = ServoPCA9685.ServoPCA9685(pca9685, PCA9685.CHANNEL00)

# 0 - > 180
for angle in range(0, 180 + 1):
    servo00.set_angle(angle)
    time.sleep(0.01)

# 180 -> 0
for angle in reversed(range(0, 180 + 1)):
    servo00.set_angle(angle)
    time.sleep(0.01)

servo00.disable()

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

Управлять несколькими сервоприводами можно аналогичным способом, как и одним. Единственное отличие в том, что нужно создать для каждого сервопривода отдельный экземпляр класса ServoPCA9685. К примеру:

servo00 = ServoPCA9685.ServoPCA9685(pca9685, PCA9685.CHANNEL00)
servo01 = ServoPCA9685.ServoPCA9685(pca9685, PCA9685.CHANNEL01)
servo02 = ServoPCA9685.ServoPCA9685(pca9685, PCA9685.CHANNEL02)
servo03 = ServoPCA9685.ServoPCA9685(pca9685, PCA9685.CHANNEL03)

каждый объект должен иметь отличное от других имя и свой собственный канал (от 0 до 15).

servo_Nx_pulse.py

Пример управления несколькими (четырьмя) сервоприводами используя метод set_pulse(pulse).

import time
import smbus
import PCA9685
import ServoPCA9685

i2cBus = smbus.SMBus(0)
pca9685 = PCA9685.PCA9685(i2cBus)
servo00 = ServoPCA9685.ServoPCA9685(pca9685, PCA9685.CHANNEL00)
servo01 = ServoPCA9685.ServoPCA9685(pca9685, PCA9685.CHANNEL01)
servo02 = ServoPCA9685.ServoPCA9685(pca9685, PCA9685.CHANNEL02)
servo03 = ServoPCA9685.ServoPCA9685(pca9685, PCA9685.CHANNEL03)

# 130 -> 510
for pulse in range(ServoPCA9685.servo_min, ServoPCA9685.servo_max + 1):
    servo00.set_pulse(pulse)
    servo01.set_pulse(pulse)
    servo02.set_pulse(pulse)
    servo03.set_pulse(pulse)
    time.sleep(0.01)

# 510 -> 130
for pulse in reversed(range(ServoPCA9685.servo_min, ServoPCA9685.servo_max + 1)):
    servo00.set_pulse(pulse)
    servo01.set_pulse(pulse)
    servo02.set_pulse(pulse)
    servo03.set_pulse(pulse)
    time.sleep(0.01)

servo00.disable()
servo01.disable()
servo02.disable()
servo03.disable()

Материалы

Все файлы в одном архиве (Скачать zip архив)
SMBus: Работа с шиной I2C на Python в Raspberry Pi/Orange Pi/Banana Pi
GitHub — adafruit/Adafruit_Python_PCA9685: Python code to use the PCA9685 PWM servo/LED controller with a Raspberry Pi or BeagleBone black.