Raspberry Pi 2 + サーボ + PS3コントローラー

Raspberry Pi 2でサーボモーターをPS3のコントローラーで制御してみた。

用意したもの

Raspberry Pi 2
サーボモーター（SG90）
マイクロサーボ９ｇＳＧ−９０: サーボ秋月電子通商電子部品ネット通販

配線

サーボモーターとRaspberry Pi 2を下記のように接続した。

サーボモーター側	Raspberry Pi 2側
橙(PWM)	Pin 12(GPIO 18)
赤(Vcc)	Pin 2(5V)
黒(GND)	Pin 6(GND)

プログラム

前回作ったDCモーター制御プログラムをサーボ制御用に少し変更しただけ。

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

#define MOTORPWM 18
#define CLOCK 400
#define RANGE 1024

#define JOY_DEV "/dev/input/js0"

using namespace std;

int main(void) {
    int i = 0;

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

    pinMode(MOTORPWM, PWM_OUTPUT);
    pwmSetMode(PWM_MODE_MS);
    pwmSetClock(CLOCK);
    pwmSetRange(RANGE);
    
    // 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];
            now_vx = (32768 + now_vx) / 720 + 24;
            printf("vx %d\n", now_vx);
            
            pwmWrite(MOTORPWM, now_vx);
        }

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