Controlling SG90 servo with PCA9685 from Python on Raspberry Pi / Orange Pi / Banana Pi

 

Controlling SG90 servo with PCA9685 from Python on Raspberry Pi / Orange Pi / Banana Pi



A servo drive is a geared motor capable of turning the output shaft to a given position (at a given angle) and holding it in this position, despite resistance and disturbances. The Tower Pro 9g SG90 servo does not have powerful specifications (only 1.2-1.6 kg * cm), but it has an inexpensive price. Great for controlling small lightweight mechanisms controlled by Arduino, Raspberry Pi controllers, etc. The operating voltage of Tower Pro 9g SG90 is from 3V to 7.2V, the rotation angle is limited to the range from 0 to 180 degrees (in reality - a little more).

Controlling SG90 servo with PCA9685 from Python on Raspberry Pi

In this tutorial, we will control an SG90 servo using a PCA9685 from Python on an Orange Pi PC . You can use a Raspberry Pi , Banana Pi , NanoPi, or any other Linux mini-computer with an I2C port.

Python library for PCA9685

Since the Adafruit_Python_PCA9685 library for working with PCA9685 from Python only works on the Raspberry Pi, it has been rewritten to be usable on the Orange Pi and Banana Pi. Now SMBus is used as I2C driver, how to install here:  SMBus: Working with I2C bus in Python in Raspberry Pi / Orange Pi / Banana Pi .

PCA9685.py file

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()

Description of methods (functions)

__init__()

Class constructor.

__init__(self, i2cBus, address=PCA9685_ADDRESS)

Parameters
i2cBus - Object of type PCA9685.
address- I2C device address. By default PCA9685_ADDRESS = 0x40.

begin()

Device initialization.

begin(self)

set_address()

Setting the device address.

set_address(self, address)

Parameters
address - I2C device address.

set_i2c_bus()

Installing the I2C bus.

set_i2c_bus(self, i2cBus)

Parameters
i2cBus - Object of type PCA9685.

set_pwm()

Sets the PWM of one of the PCA9685 pins.

set_pwm(self, channel, on, off)

Parameters
channel - One of the PWM pins from 0 to 15.
on- At what point in the cycle of 4096 parts to turn on the PWM.
off- At what point in the cycle of 4096 parts to turn off the PWM.

set_all_pwm()

Sets the PWM on all PCA9685 pins.

set_all_pwm(self, on, off)

Parameters
on - At what point in the cycle of 4096 parts to turn on the PWM.
off- At what point in the cycle of 4096 parts to turn off the PWM.

set_pwm_freq()

Sets the PWM frequency for the entire chip, up to ~ 1.6 kHz.

set_pwm_freq(self, freq_hz)

Parameters
freq_hz - Frequency in Hertz.

Python library for servos

For more convenient control of the servo, the main functions have been collected in one class -  ServoPCA9685Here you can find the minimum ( servo_min = 130) and maximum ( servo_max = 510) pulse lengths to safely control the SG90 servo.

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

If your servo works with other values, then you can edit them.

ServoPCA9685.py file

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)

Description of methods (functions)

__init__()

Class constructor.

__init__(self, pca9685, channel)

pca9685- Object of type PCA9685.
channel- One of the PWM pins of PCA9685 from 0 to 15.

set_pwm_freq()

Sets the PWM frequency for your servo.

set_pwm_freq(self, freq=50)

freq- Frequency in Hertz. By default freq=50.

set_angle()

Sets the approximate angle of the servo.

set_angle(self, angle)

angle - Angle from 0 to 180 degrees.

set_pulse()

Setting the pulse length.

set_pulse(self, pulse)

pulse - PWM pulse length.

disable()

Disabling the servo (setting the pulse length to zero "0").

disable(self)

Sample programs

Wiring diagram for SG90 servo drive to PCA9685

Wiring diagram for SG90 servo drive to PCA9685 and Orange Pi OneControl of one SG90 servo drive

To control a servo drive via PCA9685, the following steps must be followed:

  1. You need to open the I2C bus "0" (or "1");
    i2cBus = smbus.SMBus(0)
  2. We create an object of the class PCA9685, and as a parameter of the constructor we use the above created object i2cBus:;
    pca9685 = PCA9685.PCA9685(i2cBus)
  3. Create class object ServoPCA9685to control a servo, as the first parameter using the above-created object, pca9685and the second option - is the channel number PCA9685, you can select the following values: PCA9685.CHANNEL00,  PCA9685.CHANNEL01,  PCA9685.CHANNEL02, ...,  PCA9685.CHANNEL15or a number from 0 to 15;
    servo00 = ServoPCA9685.ServoPCA9685 (pca9685, PCA9685.CHANNEL00)
  4. Two methods can be used to control the servo, namely:,  set_pulse(pulse)where  pulseis the length of the PWM pulse from  servo_min = 130to  servo_max = 510and  set_angle(angle), where angleis the angle of rotation from 0 to 180 degrees, the method (function) proportionally transfers the value from the current range of values ​​in degrees (from 0 to 180) to the new range (from 130 to 510) in pulses.

The example code below turns the servo in one direction,

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

then to another

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

using the method set_pulse(pulse), and at the end turns off the PWM supplied to the servo.

servo00.disable()

Servo_1x_pulse.py file

An example of controlling a servo using the method 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 file

An example of controlling a servo using the method 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()

Controlling multiple SG90 servos

You can control several servos in the same way as one. The only difference is that you need to create a separate instance of the class for each servo  ServoPCA9685For example:

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

each object must have a different name and its own channel (from 0 to 15).

servo_Nx_pulse.py

An example of controlling several (four) servos using the method 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()
Previous Post Next Post

Contact Form