PCA9685使用のI2C接続16チャンネルサーボ＆PWM駆動キットをラズパイ3ｂに接続

PCA9685を使用したI2C接続の、16チャンネル、サーボ＆PWM駆動キットをRasperryPi3bに接続、これにHobbyWing　WP-1060を1台、FutabaのサーボモーターFP-S148を2ヶ接続するシステムを作りました。
サーボ2台までは、Raspberryで制御できるのですが、それを越えるとだめなので検討の結果このキットを使用することにしました。
PCA9685をRaspberryPiに接続した事例が見つからなかったので、メモ書き程度のつもりで、アップしました。

1．PCA9685の説明
　PCA9685はI2Cに接続し、16チャンネル(16ヶ）のサーボモーターもしくはLEDを駆動でき、1チャンネル最大25maの電流を流せる仕様になっています。複数ユニットの接続(最大62台）も可能になっているようで、各ユニットのI2Cアドレスを変えることにより（ハンダ付けにて変更）、可能だそうです。PCA9685のI2CアドレスはX040ですが、別途X0E0のアドレスがあり、このアドレスで、前ユニット・チャンネルへのコマンドを発行できるそうです。私が作ったシステムでは、I2C接続の超音波測距器を使用していて、これのI2Cアドレスも0x040だったので、このキットのアドレスを0x041に変えて使用しました。
マニュアルではLEDの接続を前提としているようで、各チャンネルをLEDｎ（n:0,1,・・・）と表記されていました。
使用法の概略は以下の通り。
①サーボモーターの周期（1回転に要する時間？プリスケール値と呼ばれています）をPCA9685に登録する。
②1周期の中でどこから駆動を開始するかをLEDｎのレジスターに書き込む
③1周期のどこで駆動を終了するかをLEDｎのレジスターに書き込む
（駆動の開始終了の信号で、パルス開始（遅延時間）と終了と呼ぶようにします）
以上です。
PCA9685は、時間をカウントで管理していて、1周期を4096カウントとし、1周期は0カウントで開始し、4095カウントで終了するようになっています。
LEDｎのレジスターは開始、終了のカウント値を持ち、各時点のカウント値を上位、下位に分けて保管します。従って、LEDｎのレジスターは4ヶで構成されています。

以下、私の作った具体的な事例を書いていきます。

2．サーボモーターFP-S148、WP-1060の説明
サーボモーターFP-S148のサーボモーターの制御上の特性は、
周期：20ms。
モーターの回転角、0度：0msより立ち上がり、0.5ms幅のパルスにより回転。
回転角、180度：0msより立ち上がり、2.5ms幅のパルスにより回転となっているよう。
（サーボモーターSG-5010の値を使って、うまくいっているので、その特性値を使用した）

WP-1060は、小型モーターのスピードコントローラーで、FP-S148に準じる扱いでOKで、初期化時に最小回転角と最大回転角を指定すれば良いようです。それで、0ｖからバッテリーの最大値までの電圧で接続してあるモーターを制御してくれるようです。

3．プリスケール値とサーボモーターの回転角（θ）を与えたときのパルス開始・終了の計算及び書き込みレジスターNo.

①プリスケール値
書き込むレジスターのアドレス：0xFE
プリスケール値の計算
プリスケール値＝(25MHZ/(4096 / サーボモーターの周期)四捨五入 – 1
25MH：PCA9685のクロックの振動数。外部から与える場合はその値。
　　　4096：PCA9685が内部で使うカウント。1サイクルのカウント数を4096。
　　　サーボモーターの周期:20ms=2 x 10^(-2)秒
これよりプリスケール値は25x10＾6/(4096/2x10^(-2))=121=0x79　となる。

②LEDｎ（サーボモーター）に回転角θを回転させる
LED0のアドレスは、0x06,0x07,0x08,0x09で与えられるので、LEDｎのアドレスは以下になる。
　　LEDn-ON-L = 0x06 + n x 4
　　LEDn-ON-H = 0x07 + n x 4
　　LEDn-OFF-L = 0x08 + n x 4
　　LEDn-OFF-H = 0x09 + n x 4
ここで、LEDn-ON-L:駆動開始パルスの低位レジスター
　　　　LEDn-ON-H:駆動開始パルスの上位レジスター
　　　　LEDn-OFF-L:駆動終了パルスの低位レジスター
　　　　LEDn-OFF-H:駆動開始パルスの上位レジスター

駆動パルス開始値：0x00（遅延時間：0）
駆動パルス終了位置の計算
回転角0度が0.5ms、180度が2.5ms。
駆動パルス開始値は、遅延時間ｔとすると、サーボモーターの周期をT（20ms）で表すと、
t/T x 4096 – 1
駆動パルス終了位置は、遅延時間　＋　負荷時間
負荷時間は角度θの回転の時間なので、負荷時間をsとすると、
s = 2ms x θ/180 + 0.5　=　θ/90＋0.5　(0.5は0度で0.5msのため)
これよりは駆動パルス終了のカウントは
(θ/90 +0.5)/T x 4096 – 1 にONのカウントを加えた値になる。
このシステムの場合は、T = 20、t = 0なので、θの回転角を与えるサーボのカウントは
{θ/90 + 0.5}/20 x 4096 -1
　= 4.096/1.8 x θ + 102.4 – 1 となる。

4．スクリプトの記述
以上の内容を踏まえ、PCA9685の制御をServoMとしてクラス化しました。
プリスケール値を書き込むとき、MODE1レジスター（アドレス：0x0）のスリープビットを1にすること、書き込み後は他のビットを含め元に戻す等の必要があるため､一連の手続きを行っている。下に記述しているスクリプトのServoMクラスの中のinitルーチンのような、記述になる。（秋月電子通商の商品に記載のArduinoライブラリーを参照した）
python3のスクリプトを以下に記載します。記述規則があるようなのですが、自己流で書いています。読みづらい点はご勘弁ください。
また、上の説明とは関係のないことですが、サーボモーターの初期角度、及びWP-1060の初期化時の最小回転角と最大回転角を指定する行があります。

I2C_ADDR_PCA9685 = 0x041
"""
class ServoM:
"""
Servo controller PC9685 制御クラス
"""
Mode1reg = 0x00
Prescreg = 0x0FE
PrescaleVal = 0x079

def __init__(self,i2c_addr = I2C_ADDR_PCA9685):
    self.i2c = pi.I2C()
    self.fd = self.i2c.setup(i2c_addr)
    self._init()

def _init(self):
    """
    PC9685初期セット
    各サーボモーターの初期角度をセット
    """
    self.i2c.writeReg8(self.fd,self.Mode1reg,0x0)

    oldrg = self.i2c.readReg8(self.fd,self.Mode1reg)
    newrg = (oldrg and 0x7f) or 0x010
    self.i2c.writeReg8(self.fd,self.Mode1reg,newrg)
    self.i2c.writeReg8(self.fd,self.Prescreg,self.PrescaleVal)
    self.i2c.writeReg8(self.fd,self.Mode1reg,oldrg)
    time.sleep(5)
    self.i2c.writeReg8(self.fd,self.Mode1reg,oldrg or 0xa1)

    self.rot_theta(SVNO_SPEED,FW_FAST)
    self.rot_theta(SVNO_SPEED,BK_FAST)
    self.rot_theta(SVNO_SPEED,SP_HALT)
    self.rot_theta(SVNO_CLUTCH,ST_MIDL)
    self.rot_theta(SVNO_FL,FL_OFF)

def rot_theta(self,n,theta):
    wk = math.ceil(float(4.096 * theta / 1.8 + 102.4)) - 1
    (hb,lb) = divmod(wk,0x0100)
    m = n * 4
    self.i2c.writeReg8(self.fd,0x06+m,0x00)
    self.i2c.writeReg8(self.fd,0x07+m,0x00)
    self.i2c.writeReg8(self.fd,0x08+m,lb)
    self.i2c.writeReg8(self.fd,0x09+m,hb)

def final(self):
    self.rot_theta(SVNO_SPEED,SP_HALT)
    self.rot_theta(SVNO_CLUTCH,ST_MIDL)
    self.rot_theta(SVNO_FL,FL_OFF)