Управление сервоприводом SG90 с помощью PCA9685 из Python на Raspberry Pi и Orange 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 нужно соблюдать следующие шаги:

Нужно открыть шину I2C «0» (или «1»);
i2cBus = smbus.SMBus(0)
Создаём объект класса PCA9685, а в качестве параметра конструктора используем выше созданный объект: i2cBus;
pca9685 = PCA9685.PCA9685(i2cBus)
Создаём объект класса ServoPCA9685 для управления одного сервопривода, в качестве первого параметра используем выше созданный объект, pca9685, а второй параметр — это номер канала PCA9685, можно выбрать следующие значения: PCA9685.CHANNEL00, PCA9685.CHANNEL01, PCA9685.CHANNEL02, …, PCA9685.CHANNEL15 или номера от 0 до 15;
servo00 = ServoPCA9685.ServoPCA9685(pca9685, PCA9685.CHANNEL00)
Для управления сервоприводом можно использовать два метода, а именно: 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()

Источник

27.02.2023 в 19:47, Просмотров: 1589
Опубликовал: ak167






Предложить пост

Поиск



По всему сайту
По тэгам
По тэгам и заголовкам

Боковое меню

  • Карта сайта
  • RSS-каналы
  • Обратная связь
  • Пользователи



    Рейтинг@Mail.ru