Raspberry Pi で Adafruit のI2C接続 16-Channel PWM/Servo Driver を試してみた。

　今回のガラクタは Adafruit の I2C 16-Channel PWM/Servo Driver です。しばらく前にスイッチさんで買って、在庫棚で忘れられて放置されていました。たまたま手にとったので、試して見ることにします。

　ものとしては、I2CインタフェースのLED用のPWMドライバ PCA9685 が載っていて、PWM信号でラジコンサーボも動かせるようにモータ電源が別に取れるように配線されています。じつはRaspberry Pi自体にもPWM出力は1chのみあるのですが、車輪型にしても最低2ch、サーボならもっと多く欲しくなります。ソフトウェアでPWMを生成することもできるかもしれませんが、このモジュールを使えば、I2Cで16ch、アドレスを変えて同じモジュールも複数つなげるみたいなので、さらに拡張もできるはずです。

　Adarfuit のサイトに Arduinoでのチュートリアル&ライブラリのダウンロードリンク と Raspberry Pi でのPythonでのチュートリアル&ライブラリのダウンロードリンクがあります。とりあえず、これらの動作を確認した後、Raspberry Pi で C++ で動かしてみることにしました。

　といっても、Arduinoのライブラリのコードをパクってきて、Raspberry Pi で動かしてみただけです。

以下、ソースです。

// Ada I2C 16chサーボドライバ

#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <math.h>
#include <linux/i2c-dev.h>
#include <fcntl.h>
#include <sys/ioctl.h>

// Ada 16ch PWMサーボドライバクラス

#define PCA9685_SUBADR1 0x2
#define PCA9685_SUBADR2 0x3
#define PCA9685_SUBADR3 0x4

#define PCA9685_MODE1 0x0
#define PCA9685_PRESCALE 0xFE

#define LED0_ON_L 0x6
#define LED0_ON_H 0x7
#define LED0_OFF_L 0x8
#define LED0_OFF_H 0x9

#define ALLLED_ON_L 0xFA
#define ALLLED_ON_H 0xFB
#define ALLLED_OFF_L 0xFC
#define ALLLED_OFF_H 0xFD

class Ada_ServoDriver
{
public:
 Ada_ServoDriver(int i2c);
 void reset(void);
 void setPWMFreq(float frea);
 void setPWM(uint8_t srvNo, uint16_t onTime, uint16_t offTime);
 void setServoPulse(uint8_t ch, double pulseWidth_us);
 uint8_t read8(uint8_t addr);
 void write8(uint8_t addr, uint8_t d);
 
private:
 uint8_t _i2cAddr;
 int _i2c;
};


Ada_ServoDriver::Ada_ServoDriver(int i2c)
{
 _i2c = i2c;
}

void Ada_ServoDriver::write8(uint8_t addr, uint8_t d)
{
 uint8_t sendData[2];
 
 sendData[0] = addr;
 sendData[1] = d;
 if (write(_i2c, sendData, 2) != 2)
 {
  printf("Faild to send i2c\n");
 }
}

uint8_t Ada_ServoDriver::read8(uint8_t addr)
{
 uint8_t sendData;
 uint8_t readData;
 
 sendData = addr;
 if (write(_i2c, &sendData, 1) != 1)
 {
  printf("Failed to send i2c @read\n");
 }
 else
 {
  if (read(_i2c, &readData, 1) != 1)
  {
   printf("Failed to read i2c\n");
  }
 }
 
 return readData;
}

void Ada_ServoDriver::reset(void)
{
 write8(PCA9685_MODE1, 0x0);
}

void Ada_ServoDriver::setPWMFreq(float freq)
{
 float prescaleval = 25000000;
 
 prescaleval /= 4096;
 prescaleval /= freq;
 prescaleval -= 1;
 printf("Estimated pre-scale: %f\n", prescaleval);
 
 uint8_t prescale = floor(prescaleval + 0.5);
 printf("Final pre-scale: %d\n", prescale); 
 
 uint8_t oldmode = read8(PCA9685_MODE1);
 uint8_t newmode = (oldmode&0x7F) | 0x10; 
 write8(PCA9685_MODE1, newmode); 
 write8(PCA9685_PRESCALE, prescale);
 write8(PCA9685_MODE1, oldmode);
 sleep(5);
 write8(PCA9685_MODE1, oldmode | 0xa1);  
}

void Ada_ServoDriver::setPWM(uint8_t srvNo, uint16_t onTime, uint16_t offTime)
{
 uint8_t sendData[5];
 
 sendData[0] = LED0_ON_L + 4 * srvNo;
 sendData[1] = (uint8_t)(0x00ff & onTime);
 sendData[2] = (uint8_t)((0xff00 & onTime) >> 8);
 sendData[3] = (uint8_t)(0x00ff & offTime);
 sendData[4] = (uint8_t)((0xff00 & offTime) >> 8);
 
 if (write(_i2c, sendData, 5) != 5)
 {
  printf("Faild to send i2c @setPWM\n");
 }
}

#define SERVO_CONTROL_FREQUENCY 60
#define SERVO_CENTER_PULSE_WIDTH_US 1520
#define SERVO_CENTER_PULSE_WIDTH_US_FUTABA_OLD  1310
#define SERVO_RANGE_PULSE_WIDTH_US 1600

void Ada_ServoDriver::setServoPulse(uint8_t ch, double pulseWidth_us)
{
 double pulselength;
 double pulseWidth;
 
 // 1秒=1000000usを60Hzで割ってパルス長を算出。
 pulselength = 1000000 / SERVO_CONTROL_FREQUENCY;
 // 12bit(2^12=4096)分解能相当へ。1分解能当たりの時間算出。
 pulselength /= 4096;
 // PWMのパルス設定値を算出。
 pulseWidth = pulseWidth_us / pulselength;
 
 // PWM値設定。
 //  setPWM(channel, on_timing, off_timing)
 //  channelで指定したチャネルのPWM出力のon(0→1）になるタイミングと
 //  off(1→0)になるタイミングを0～4095で設定する。
 setPWM(ch, 0, pulseWidth);
}

int main(int argc, char **argv)
{
 int i2c;    // ファイルディスクリプタ
 char *i2cFileName = "/dev/i2c-1"; // I2Cデバイスのパス（古いものはi2c-0）
 int driverAddress = 0x40;
 
 //
 if ((i2c = open(i2cFileName, O_RDWR)) < 0)
 {
  printf("Faild to open i2c port\n");
  exit(1);
 }
 
 //
 if (ioctl(i2c, I2C_SLAVE, driverAddress) < 0)
 {
  printf("Unable to get bus access to talk to slave\n");
  exit(1);
 }
 
 Ada_ServoDriver servo(i2c);
 
 servo.reset();
 
 usleep(100000);
 
 // サーボ制御パルス周波数の設定。
 servo.setPWMFreq(SERVO_CONTROL_FREQUENCY);
 
 // サーボをセンタ位置へ。
 servo.setServoPulse(0, SERVO_CENTER_PULSE_WIDTH_US);
 servo.setServoPulse(1, SERVO_CENTER_PULSE_WIDTH_US);
 servo.setServoPulse(2, SERVO_CENTER_PULSE_WIDTH_US);
 servo.setServoPulse(3, SERVO_CENTER_PULSE_WIDTH_US);
 
 
 sleep(1);
 
 // とりあえず、適当に動かしてみる。
 servo.setServoPulse(1, (SERVO_CENTER_PULSE_WIDTH_US - SERVO_RANGE_PULSE_WIDTH_US / 4));
 
 while(true)
 {
  
  servo.setServoPulse(0, (SERVO_CENTER_PULSE_WIDTH_US + SERVO_RANGE_PULSE_WIDTH_US / 4));
  servo.setServoPulse(3, (SERVO_CENTER_PULSE_WIDTH_US - SERVO_RANGE_PULSE_WIDTH_US / 4));
  
  usleep(500000);
  
  servo.setServoPulse(1, (SERVO_CENTER_PULSE_WIDTH_US - SERVO_RANGE_PULSE_WIDTH_US / 4));
  servo.setServoPulse(2, (SERVO_CENTER_PULSE_WIDTH_US - SERVO_RANGE_PULSE_WIDTH_US / 4));
  
  usleep(500000);
  
  servo.setServoPulse(0, (SERVO_CENTER_PULSE_WIDTH_US - SERVO_RANGE_PULSE_WIDTH_US / 4));
  servo.setServoPulse(3, SERVO_CENTER_PULSE_WIDTH_US);
  
  usleep(500000);
  
  servo.setServoPulse(1, SERVO_CENTER_PULSE_WIDTH_US);
  servo.setServoPulse(2, (SERVO_CENTER_PULSE_WIDTH_US + SERVO_RANGE_PULSE_WIDTH_US / 4));
  
  usleep(500000);
 }
 
 return 0;
}