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 &
すると再接続できた!