Raspberry Pi 2でDCモーターを２つ制御する

Raspberry Pi 2でDCモーターを２つ制御してみた。
ついでにRaspberry Pi 2をモバイルバッテリーで動かしてみたり、モーターを変えたり、ギアボックスを付けてみたりした。

用意したもの

回路

以前作ったモータードライバーとDCモーターの回路をもう一つ作るだけだが、配線がかなりカオスになった…。
また、モーターがRA-280になり、必要な電圧が1.5vから3vに上がったので、単3電池を4本繋ぐことにした。
3vに対して6v近い電圧を掛けることになるが、それくらいの電圧を掛けてもモーターは簡単には壊れないらしいのでそのままにしている。

f:id:kaiware007:20150715002334p:plain

プログラム

例によってPS3コントローラーでモーターを制御する。
今回はモーターが２つあるので、左右のアナログスティックでそれぞれ別々のモーターを制御するようにした。
モーター2つを制御するということで、GPIOピンが４つとPWM制御ピンが２つ必要になるが、Raspberry PiのPWM制御が出来るピンは決まっているらしく、Pi 2の場合、GPIO12とGPIO18は出来ることがわかった。
最初はピンが決まってると知らずに適当なGPIOに繋いだらモーターが回転しなかったので焦った。

#include <vector>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <linux/joystick.h>
#include <wiringPi.h>
#include <unistd.h>
#include <cstdio>
#include <cmath>

#define MOTOROUT1_1 14
#define MOTOROUT1_2 15
#define MOTORPWM1 18

#define MOTOROUT2_1 23
#define MOTOROUT2_2 24
#define MOTORPWM2 12

#define POW1 1024
#define POW2 768

#define JOY_DEV "/dev/input/js0"

using namespace std;

void motorControl(int now, int old, int pwmNo, int gpioNo1, int gpioNo2) {
    if ( ((old < 0) ^ (now < 0)) != 0 ) {
        // 回転方向が変わるときは一瞬停止しないと壊れる  
        pwmWrite(pwmNo, 0);
        digitalWrite(gpioNo1, 0);
        digitalWrite(gpioNo2, 0);
        //printf("STOP\n");
        usleep(50);
    }

    if ( now != 0 ) {
        bool bit = (now > 0 );
        digitalWrite(gpioNo1, (int)bit);
        digitalWrite(gpioNo2, (int)(!bit));
        pwmWrite(pwmNo, abs(now));
        //printf("PWM %d\n", now);
    }else if(( now == 0 )&&( old != 0 )){
        pwmWrite(pwmNo, 0);
        digitalWrite(gpioNo1, 0);
        digitalWrite(gpioNo2, 0);
    }
}

int main(void) {
    int i = 0;
    int old_vx = 0;

    // Initialize GPIO
    if ( wiringPiSetupGpio() == -1) {
        printf("setup error");
        return -1;
    }

    pinMode(MOTOROUT1_1, OUTPUT);
    pinMode(MOTOROUT1_2, OUTPUT);
    pinMode(MOTORPWM1, PWM_OUTPUT);

    pinMode(MOTOROUT2_1, OUTPUT);
    pinMode(MOTOROUT2_2, OUTPUT);
    pinMode(MOTORPWM2, PWM_OUTPUT);

    digitalWrite(MOTOROUT1_1, 0);
    digitalWrite(MOTOROUT1_2, 0);
    digitalWrite(MOTORPWM1, 0);

    digitalWrite(MOTOROUT2_1, 0);
    digitalWrite(MOTOROUT2_2, 0);
    digitalWrite(MOTORPWM2, 0);
    
    // Initialize Joystick
    int joy_fd = -1;
    int num_of_axis = 0;
    int num_of_buttons = 0;

    char name_of_joystick[80];
    vector<char> joy_button, old_button;
    vector<int> joy_axis, old_axis;
    
    if((joy_fd = open(JOY_DEV, O_RDONLY)) < 0) {
        printf("Failed to open %s", JOY_DEV);
        return -1;
    }

    ioctl(joy_fd, JSIOCGAXES, &num_of_axis);
    ioctl(joy_fd, JSIOCGBUTTONS, &num_of_buttons);
    ioctl(joy_fd, JSIOCGNAME(80), &name_of_joystick);

    joy_button.resize(num_of_buttons, 0);
    joy_axis.resize(num_of_axis, 0);
    
    old_button.resize(num_of_buttons, 0);
    old_axis.resize(num_of_axis, 0);
    

    printf("Joystick: %s axis: %d buttons: %d\n", name_of_joystick, num_of_axis, num_of_buttons);

    fcntl(joy_fd, F_SETFL, O_NONBLOCK); // using non-blocking mode

    while(true) {
        js_event js;

        read(joy_fd, &js, sizeof(js_event));
        
        switch(js.type & ~JS_EVENT_INIT) {
            case JS_EVENT_AXIS:
                joy_axis[(int)js.number] = js.value;
                break;
            case JS_EVENT_BUTTON:
                joy_button[(int)js.number] = js.value;
                break;
        }

        if ( num_of_axis > 3 ) {
            int now_lx = joy_axis[0] / 32;
            int now_ly = joy_axis[1] / 32;
            int now_rx = joy_axis[2] / 32;
            int now_ry = joy_axis[3] / 32;

            int old_lx = old_axis[0] / 32;
            int old_ly = old_axis[1] / 32;
            int old_rx = old_axis[2] / 32;
            int old_ry = old_axis[3] / 32;

            motorControl(now_ly, old_ly, MOTORPWM1, MOTOROUT1_1, MOTOROUT1_2);
            motorControl(now_ry, old_ry, MOTORPWM2, MOTOROUT2_1, MOTOROUT2_2);
            
            for ( int i = 0; i < num_of_axis; i++ ) {
                old_axis[i] = joy_axis[i];
            }
            for ( int i = 0; i < num_of_buttons; i++ ) {
                old_button[i] = joy_button[i];
            }

        }

        usleep(100);
    }
    
    close(joy_fd);
    return 0;
}

動作確認

f:id:kaiware007:20150715003545j:plain

左右のアナログスティックで２つのモーターが制御できた。
これで戦車とか作れる！

おまけ１（ハイパワーギアボックス）

以前のFA-130モーターは明らかにパワー不足な感じだったので、２ランク位上のRA-280モーターに変えた。
将来的にRaspberry Pi を積んだラジコン的なモノを作ろうと思っているので、更にトルクを上げるためにタミヤのハイパワーギアボックスを導入してみた。
タミヤのハイパワーギアボックスHEには標準で140系モーターが付属していたが、RA-280に変えてみた。しかし、モーターの奥行き（？）が数ミリ長かったため、ピッタリふたが閉まらない…。
無理矢理長めのネジで止めて何とか固定できた。
とりあえずギア比は40：1にした。
まだタイヤがないので実際に乗せて動かしたりはできないが…。

おまけ２（モバイルバッテリー）

Raspberry Pi 2の電源をコンセントから供給した状態でモーターを回すとPWR LEDが消えて焦った。
モーターの電源自体は単３電池４本で供給しているので、モーターのせいではないはず…。
USBにWiFiアダプタやBluetoothアダプタをつけてるせいだろうか？

試しに、Raspberry Pi 2側の電源をモバイルバッテリーに変えてみたら、逆にPWR LEDが消えずに安定した。
コンセント側のUSB変換アダプタの電流が低いのかもしれない（もしくはケーブル？）
電源が安定したので調子に乗ってWebカメラも繋いでみたが、こちらも問題なく動作した。
モバイルバッテリー凄い。