@silver2row
I have been playing around with the python and hooked up a servo I found.
I have added a couple of functions ( set_pwm_frequency & set_servo_angle)
The code assumes that a 2ms pulse is 180 degrees. You would need to check what servos you are using to see what angle of rotation they have. My servos are only 90 degrees at 2ms.
Don’t forget gpio68 needs to be low for the PWM’s to work.
from smbus2 import SMBus
from time import sleep
# registers!
Mode1 = 0x00
Mode2 = 0x01
FIRST_SERVO = 0x06
All_LED = 0xFA
PRE_SCALE = 0xFE
DEFAULT_FREQUENCY = 200
AUTO_INC = (1 << 5)
SLEEP = (1 << 4)
class PCA9685:
def __init__(self, bus, addr):
self.addr = addr
self.bus = bus
self.write_reg(Mode1, AUTO_INC)
self.frequency = DEFAULT_FREQUENCY
self.bit_time = 1.0/ ( self.frequency*4096.0)
sleep(500e-6)
def read_reg(self, reg):
assert reg in range(0, 256)
return self.read_regs(reg, 1)[0]
def write_reg(self, reg, value):
return self.write_regs(reg, [value])
def read_regs(self, reg, value):
assert reg in range(0, 256)
assert value in range(1, 257 - reg)
return self.bus.read_i2c_block_data(self.addr, reg, value)
def write_regs(self, reg, values):
assert reg in range(0, 256)
return self.bus.write_i2c_block_data(self.addr, reg, values)
def set_pwm_frequency(self, frequency):
# the +9 is a bit of a hack to get very nearly 50Hz. For other frequencies this would be wrong
# at 60Hz it gives around 58Hz
val = int( round( 25000000/( 4096*frequency ))-1)+9
self.frequency = frequency
self.bit_time = 1.0/ ( self.frequency*4096.0)
self.write_reg(0, AUTO_INC | SLEEP)
self.write_reg(254,val)
self.write_reg(0, AUTO_INC )
def set_servo_angle(self,servo,angle):
# offset stagers the PWMs so each channel starts at a different time
# you could just leave this at 0 if you wish
offset = int(servo *200)
pwm_high = int( (( (angle/180)*0.001 + 0.001 )/ self.bit_time))
pwm_start = offset
pwm_end = int( offset + pwm_high ) % 4096
on_l = pwm_start & 0xff
on_h = pwm_start >> 8
off_l = pwm_end & 0xff
off_h = pwm_end >> 8
values=[on_l, on_h, off_l, off_h]
reg = FIRST_SERVO + 4 * servo
self.write_regs(reg, values)
def get_pwm(self, output):
assert output in range(0, 16)
reg = FIRST_SERVO + 4 * output
[on_l, on_h, off_l, off_h] = self.read_regs(reg, 4)
on = on_l | on_h << 8
off = off_l | off_h << 8
phase = on
duty = (off - on) & 0xfff
if off & 0x1000:
duty = 0
elif on & 0x1000:
duty = 4096
return (duty, phase)
def set_pwm(self, output, duty, phase = 0):
assert duty in range(0, 4097)
assert phase in range(0, 4096)
if output == "all":
reg = All_LED
else:
assert output in range(0, 16)
reg = LED +4 * output
on = phase
off = (duty + phase) & 0xfff
if duty == 0:
off |= 0x1000
elif duty == 4096:
on |= 0x1000
on_l = on & 0xff
on_h = on >> 8
off_l = off & 0xff
off_h = off >> 8
self.write_regs(reg, [on_l, on_h, off_l, off_h])
and some test code,
pwm_controller.set_pwm_frequency(50)
for l in range(0,5):
pwm_controller.set_servo_angle(0,0)
pwm_controller.set_servo_angle(15,0)
sleep(1)
pwm_controller.set_servo_angle(0,90)
pwm_controller.set_servo_angle(15,90)
sleep(1)
pwm_controller.set_servo_angle(0,180)
pwm_controller.set_servo_angle(15,180)
sleep(1)