カイワレスタイル

ゲーム、アニメ、プログラム、興味のあることをツラツラと。

Raspberry Pi 2とPS3のコントローラーでモーターを制御する

前回の続き。
Raspberry Pi 2とPS3のコントローラーをBluetooth接続できたので、
コントローラーでDCモーターの回転方向を制御してみた。


用意したもの

  • Raspberry Pi 2
  • PS3のコントローラー
  • USB Bluetoothドングル(BSBT4D09)
  • モータードライバ(TA7291P)
  • DCモーター(FA-130RA-2270)
  • 単3電池2本

いきなりプログラミング

前回と前々回で必要なソフトやライブラリのインストールは済んでいるので、プログラムを書くだけ。
PS3コントローラーの左スティックを左右に傾けると、それに合わせてモーターの回転方向と回転速度を変えるプログラムを作成。

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

#define MOTOROUT1 14
#define MOTOROUT2 15
#define MOTORPWM 18
#define POW1 1024
#define POW2 768

#define JOY_DEV "/dev/input/js0"

using namespace std;

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

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

    pinMode(MOTOROUT1, OUTPUT);
    pinMode(MOTOROUT2, OUTPUT);
    pinMode(MOTORPWM, PWM_OUTPUT);

    digitalWrite(MOTOROUT1, 0);
    digitalWrite(MOTOROUT2, 0);
    digitalWrite(MOTORPWM, 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;
    vector<int> joy_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);
    
    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 > 0 ) {
            int now_vx = joy_axis[0] / 32;
            
            printf("vx %d old %d\n", now_vx, old_vx);
            
            if ( ((old_vx < 0) ^ (now_vx < 0)) != 0 ) {
                // 回転方向が変わるときは一瞬停止しないと壊れる
                pwmWrite(MOTORPWM, 0);
                digitalWrite(MOTOROUT1, 0);
                digitalWrite(MOTOROUT2, 0);
                usleep(50);
            }

            if ( now_vx != 0 ) {
                bool bit = (now_vx > 0 );
                digitalWrite(MOTOROUT1, (int)bit);
                digitalWrite(MOTOROUT2, (int)(!bit));
                pwmWrite(MOTORPWM, now_vx);
            }   
            old_vx = now_vx;
        }

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

ビルドして実行。

g++ -o motor02 motor02.cpp -lwiringPi
sudo ./motor02

操作に合わせてモーターの回転方向が変わるのを確認できた。
一応スティックの方向け加減で出力も変わるようになっているが、電圧が低いためかすぐ回転が止まってしまう。


再接続の方法

前回、一度切断すると繋がらなくなると言っていたが、あれはBluetoothドングルがスキャンモードになってなかっただけだった。

sudo hciconfig hci0 piscan

で再スキャンさせると、Bluetoothドングルの青LEDが点滅するので、その状態で

sudo sixad -start &

すると再接続できた!