raspberry piでサーボモーターを動かしてみる

久しぶりにraspberry piを動かしてみる。
今回はサーボモーター制御に挑戦。

ちょっと実験死体だけなのでサーボモーターは安いものを
物色した結果、AmazonからSG90を購入。
2個で\980 でとってもリーズナブル。


サーボモーターは信号のONの時間により位置合わせをしてくれるモーターで、
今回のSG90は 回転角 0°～180°で対応する信号幅は
0.5ms～2.5msである。

本当はPWMを使うといいのだが、とりあえずソフトウェアでGPIOを叩いて

制御してみることにする

前にLED点滅制御で作ったプログラムをちょっと変更して

作ってみる

なんか動きがぎこちないがとりあえず思ったとおりに動いた。

MotionのWebカメラ制御と組み合わせて、ネットワーク越しに

カメラの向きを制御できると面白そうだ。

ソース・コード

------------------------------------------------------------------

#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <sys/mman.h>
#include <unistd.h>

//  レジスタブロックの物理アドレス
#define PERI_BASE     0x20000000
#define GPIO_BASE     (PERI_BASE + 0x200000)
#define BLOCK_SIZE    4096
//  ピン機能（BCM2835）
#define GPIO_INPUT    0x0       //  入力
#define GPIO_OUTPUT   0x1       //  出力
#define GPIO_ALT0     0x4
#define GPIO_ALT1     0x5
#define GPIO_ALT2     0x6
#define GPIO_ALT3     0x7
#define GPIO_ALT4     0x3
#define GPIO_ALT5     0x2

//  gpio[n]: GPIO 関連レジスタ (volatile＝最適化によって消滅させない→必ず実メモリにアクセスさせる)
static volatile unsigned int *gpio;
//  gpio_init: GPIO 初期化（最初に１度だけ呼び出すこと）
void gpio_init ()
{
    int fd;
    void *gpio_map;
    //  /dev/mem（物理メモリデバイス）を開く（sudo が必要）
    fd = open("/dev/mem", O_RDWR | O_SYNC);
    if (fd == -1) {
        printf("error: cannot open /dev/mem (gpio_setup)\n");
        exit(-1);
    }
    //  mmap で GPIO（物理メモリ）を gpio_map（仮想メモリ）に対応づける
    gpio_map = mmap(NULL, BLOCK_SIZE,
                    PROT_READ | PROT_WRITE, MAP_SHARED,
                    fd, GPIO_BASE );
    if ((int) gpio_map == -1) {
        printf("error: cannot map /dev/mem on the memory (gpio_setup)\n");
        exit(-1);
    }
    //  mmap 後は不要な fd をクローズ
    close(fd);
    //  gpio[index]: 整数 uint32 の配列としてレジスタへのアクセスを確立
    gpio = (unsigned int *) gpio_map;
}
//  gpio_configure: ピン機能を設定する（ピンを使用する前に必ず設定）
//      pin : (P1) 2,3,4,7,8,9,10,11,14,15,17,18,22,23,24,25,27
//            (P5) 28,29,30,31
//      mode: GPIO_INPUT, _OUTPUT, _ALT0, _ALT1, _ALT2, _ALT3, _ALT4, _ALT5
void gpio_configure (int pin, int mode)
{
    //  ピン番号チェック
    if (pin < 0 || pin > 31) {
        printf("error: pin number out of range (gpio_configure)\n");
        exit(-1);
    }
    //  レジスタ番号（index）と３ビットマスクを生成
    int index = pin / 10;
    unsigned int mask = ~(0x7 << ((pin % 10) * 3));
    //  GPFSEL0/1 の該当する FSEL (3bit) のみを書き換え
    gpio[index] = (gpio[index] & mask) | ((mode & 0x7) << ((pin % 10) * 3));
}
//  gpio_set/clear: ピンをセット (3.3V)，クリア (0V)
void gpio_set (int pin)
{
    //  ピン番号チェック（スピードを追求するなら省略してもよい）
    if (pin < 0 || pin > 31) {
        printf("error: pin number out of range (gpio_set)\n");
        exit(-1);
    }
    //  ピンに１を出力（3.3V 出力）
    gpio[7] = 0x1 << pin;   //  GPSET0
}
void gpio_clear (int pin)
{
    //  ピン番号チェック（スピードを追求するなら省略してもよい）
    if (pin < 0 || pin > 31) {
        printf("error: pin number out of range (gpio_clear)\n");
        exit(-1);
    }
    //  ピンに０を出力（0V 出力）
    gpio[10] = 0x1 << pin;  //  GPCLR0
}
#define SERVO01 17
#define SERVO02 18
#define INTERVAL_TIME 22000
#define SERVO_MOVE_MAX 180
#define SERVO_DEGREE_0_TIME 500
static void servo(unsigned long int PinNo,unsigned long int Angle);
static void servo(unsigned long int PinNo,unsigned long int Angle)
{

unsigned long int Time_H;
unsigned long int Time_L;

/* 制御角度は0～180度に限定 */
Angle = Angle % (SERVO_MOVE_MAX+1);


Time_H = SERVO_DEGREE_0_TIME+((Angle*2000)/SERVO_MOVE_MAX);
Time_L = INTERVAL_TIME - Time_H;

printf("PinNo(%02d):Time:%09d[us]\n",PinNo,Time_H);

gpio_set(PinNo);
    usleep(Time_H);
gpio_clear(PinNo);
    usleep(Time_L);
}
#define WAIT_TIME (2*1000*1000)
int main ()
{
signed long int j;

gpio_init();                        //  オマジナイ
    gpio_configure(SERVO01, GPIO_OUTPUT);
    gpio_configure(SERVO02, GPIO_OUTPUT);


/*
[INITIALIZE]
角度0℃に設定
*/
printf("TEST000\n");
for(j=0;j<10;j++){
servo(SERVO01,0);
servo(SERVO02,0);
}
usleep(WAIT_TIME);

while (1) {

/*
0->90へ移動
*/

printf("TEST 0->90\n");
for( j=0;j<=90;j=j+2){
servo(SERVO01,j);
servo(SERVO02,j);
}
usleep(WAIT_TIME);

/*
90->180へ移動
*/
printf("TEST 90->180\n");
for( j=90;j<=180;j=j+2){
servo(SERVO01,j);
servo(SERVO02,j);
}
usleep(WAIT_TIME);


/*
180->90へ移動
*/
printf("TEST 180->90\n");
for( j=180;j>=90;j=j-2){
servo(SERVO01,j);
servo(SERVO02,j);
}
usleep(WAIT_TIME);


/*
90->0へ移動
*/
printf("TEST 90->0\n");
for( j=90;j>0;j=j-2){
servo(SERVO01,j);
servo(SERVO02,j);
}
usleep(WAIT_TIME);
}
return 0;
}
------------------------------------------------------------------