ものとしては、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; }
0 件のコメント:
コメントを投稿