16チャンネルPWMコントローラーPCA9685キットのサーボモーター制御プログラム例

●使用したハードウェア

●PWMコントローラーへの外部電源の接続

サーボモーター用電源は、Raspberry Piの5V電源を使うことも可能だが、電流容量を必要とするサーボモーターや、複数台のサーボモーターを使う場合には、電流容量の大きい外部電源を使う必要がある。制御電源はRaspberry Piの3V電源、I2C通信のための、SDA、SCLを接続する。

●PWMの発生方法

PCA8685自体は外部クロックを使えるが、このPWM駆動キットは25MHzの内部オシレーターを使って矩形波を発生させている。PWMアウトプット周波数は、次の計算式で計算したprescale valueを、PRE_SCALEレジスターにセットすると得られる。ここで低い周波数の場合、正確な周波数を得るためには、オシロスコープをモニターして、最後の-1を、0～+7に変える必要がある。

　\( Prescale Value = round \left( \displaystyle \frac{osc\_clock}{4096 \times update\_rate} \displaystyle \right) – 1\)
　　　　\(osc\_clock\) : 内部発信器周波数
　　　　\(update\_rate\) : PWMアウトプット周波数

Default値は、update_rate=200Hzで、prescale valueは30(0x1E)となっている。
そのアウトプット周波数の１サイクルを4096分割し、LEDn_ONとLEDn_OFFを12bits値で指定し、必要なデューティ比の波形を生成する。このICは本来LEDをドライブするためのもので、ONとOFFの決め方で様々な波形を生成することができるが、サーボモーター駆動のための波形生成は、LEDn_ON=0として、LEDn_OFFタイミングだけを変えれば得られる。

●PWM駆動モジュール

１個のPWM駆動キットで16チャンネルのPWMを発生できる。I2Cアドレスを変えることで、最大62ユニット(992チャンネル)を連結することが可能となるが、ここでは１ユニット(16チャンネル)だけ使用する。

①init()
DefaultのMODE1レジスタ値に対して、Register Auto-Increment Enable(bit5=1)を設定し、他のI2Cと競合しないように、ALL CALLの I2C-Busアドレス(0x70)を無効(bit0=0)にしている。

②setPwmFreq(freq)
PWMアウトプット周波数(Hz)をセットする。サーボモーター(DS3218)の場合は50(Hz)をセット。設定可能な周波数は、24Hz～1526Hz（pre_scale = 0xFF～0x03）

③clear()
16ch全てのON、OFFを「0」にする。

④setPWM(ch, duty)
chのデューティ比（duty/4096）となるOFFタイミング(0～4095)をセットする。ch:0～15、duty:0～4095。このモジュールの実行時間は、約0.6msec。

⑤setPWMblock(block, dutyList)
4chのブロック毎にデューティ比（duty/4096）となるOFFタイミング(0～4095)をセットする。block:0～3、dutyList:[0～4095, 0～4095, 0～4095, 0～4095]。このモジュールの実行時間は、約2.6msec。1ch当たりの実行時間は0.65msecで、④setPWMとほぼ同じ。サーボモータ(DS3218)の速度0.16sec/60degなので、複数のモーターにPWM信号を送った場合、0.2deg/0.6msecずつ遅れて起動することになる。
（蛇足説明）
ここではwrite_i2c_block_data関数を使って、各chのOFF時間のみを２バイト連続（各ch間は不連続）で書き込みを行っているが、各chのON時間とOFF時間の連続する4バイトを、4ch分16バイトを連続してwrite_i2c_block_dataで書き込んでも、1ch当たりの実行時間はほぼ同じになる。連続するバイト数が多いほど書込み効率が良いが、書込み不要なデータが半分含まれてしまうので、実質的な書込み効率は変わらなくなるからだ。

⑥readPwmValues(chList)
chListで指定したchの12bitsのOFFタイミング(0～4095)を読み出す。リターン値は、[ch, duty(0～4095)]のリスト値。

●PWM9685制御モジュールとプログラムサンプル

・PWM9685制御モジュール(PWM9685.py)

# -*- coding: utf-8 -*-
import smbus
import time

I2C = smbus.SMBus(1)
ADR9685 = 0x40		# I2C address of PCA9685 PWM Control board
MODE1 = 0x00	# Reg. address of PCA9685 MODE1
MODE1_DEFAULT = 0x11	# Default Data of MODE1 Reg. 
PRE_SCALE = 0xFE	# Reg. address of PCA9685 prescale
PWM_DATA = 0x06 # Reg. Address of first PWM ON time
PWM_OFF_DATA = 0x08 # Reg. Address of first PWM OFF time
OSC_CLOCK = 25000000	# internal clock = 25Mhz

def init():
    setData = (MODE1_DEFAULT | 0x20) & 0xE0
        # Bit5=1: Auto increment
        # Bit4=0: Normal, oscillator ON
        # Bit3~0=0: Other I2C not responsed
    I2C.write_byte_data(ADR9685, MODE1, setData)

def setPwmFreq(freq):
    if freq < 24:
        freq = 24
    if freq > 1526:
        freq = 1526
    preScale = int(round(OSC_CLOCK/4096/freq) - 1)
    oldReg = I2C.read_byte_data(ADR9685, MODE1)
    newReg = oldReg | 0x10		# Sleep bit4=1:oscillator OFF
    I2C.write_byte_data(ADR9685, MODE1, newReg)	# go to sleep
    I2C.write_byte_data(ADR9685, PRE_SCALE, preScale)	# set prescale
    time.sleep(0.01)
    I2C.write_byte_data(ADR9685, MODE1, oldReg)
    time.sleep(0.01)

def clear():
    for ch in range(16):
        chAdr = PWM_DATA + 4 * ch	# ch = 0 to 15 (each 4 bytes)
        I2C.write_i2c_block_data (ADR9685, chAdr, [0, 0, 0, 0])

def setPWM(ch, duty):
    #ch = 0 to 15, duty = 0 to 4095
    chAdr = PWM_OFF_DATA + 4 * ch	# ch = 0 to 15 (each 4 bytes)
    outVals = [duty & 0xFF, (duty >> 8) & 0x0F]
    I2C.write_i2c_block_data (ADR9685, chAdr, outVals)

def setPWMblock(block, dutyList):
    #block = 0 to 3 (each block: 4ch), duty = 0 to 4095
    blockAdr = PWM_OFF_DATA + block * 16	#  block = 4ch x 4bytes
    for ch in range(4):
        chAdr = blockAdr + ch * 4	#  block = 4ch x 4bytes
        outVals = [dutyList[ch] & 0xFF, (dutyList[ch] >> 8) & 0x0F]
        I2C.write_i2c_block_data (ADR9685, chAdr, outVals)

def readPwmValues(chList):
    # chList=[ch No, ch No-n, .....] readするch No リスト
    pwmList = []
    for i in range(len(chList)):
        chAdr = PWM_OFF_DATA + 4 * chList[i]	# ch = 0 to 15 (each 4 bytes)
        inVals = I2C.read_i2c_block_data (ADR9685, chAdr, 2)
        pwmList.append([chList[i], inVals[0] + ((inVals[1] & 0x0F) << 8)])
            # pwmList = [[ch0, pwm0], [ch1, pwm1], ....]
    return pwmList 


if __name__ == '__main__':
    import time
    import PWM9685 as PWM
    
    PWM.init()
    PWM.setPwmFreq(50)	# 50Hz for Servo Motor(DS3218MG)
    PWM.clear()

    #Servo Motor Moves from 0deg to 270deg
        # 	duty= 2.5% to 12.5% (=102 to 512)
    PWM.setPWM(0, 102)	# move to 0 deg
    time.sleep(0.5)
    for duty in range(102, 513, 10):	# move 0 to 270 deg
        PWM.setPWM(0, duty)
        time.sleep(0.1)
    PWM.setPWM(0, 102)

    # 4 motors :  block 0 (ch.0 to ch.4)
    PWM.setPWMblock(0, [102, 102, 102, 102])
    time.sleep(0.5)
    for duty in range(102, 513, 10):
        PWM.setPWMblock(0, [duty, duty, duty, duty])
        pwmList = PWM.readPwmValues([0,1,2,3])
        strPWM = "ch-duty "
        for i in range(len(pwmList)):
            strPWM = strPWM + "{:2d}-{:03x}    ".format(pwmList[i][0], pwmList[i][1])
        print(strPWM)
    PWM.setPWMblock(0, [102, 102, 102, 102])