Raspberry Piで、OpenCV タンク(自走式)を作る

OpenCVタンク

Raspberry Piを脳みそに、自走する機械を作った。
人間の顔を認識し、その顔に狙いを付け、
その後、顔の大きさが適切になるまで、前進/後進を行う。

OpenCVが動作しているので、非常に、極めて簡単にできた。

ちなみに、これが、Raspiで、OpenCVを試した日記

できあがりは、こんな感じ。

搭載してあるものは

Raspi本体 (SDHCカード8GB挿入済み)
モータ・ドライバ基板
USB Webカメラ
WiFIドングル(USB接続)
単三電池×4
DCモータ×2

ロジック回路の電源は、外部からケーブルで、5V を供給している。
ロジック回路の電源も、モータと同じ電池から、取ってもいいのだが…
現在は、電池の消耗具合が不明なので、とりあえず、外部電源。

土台になっている、シャーシは、タミヤの楽しい工作シリーズ No.104のブルドーザ。
ちなみに、このセットのリモコンは、バネの力で、中立に戻ったりして、なかなかいい感じだ。
僕が子供の時の、タミヤ製 1/35 タイガーI型のリモコンよりかなりいいぞ。

近頃は、ロボットを動かすのに、ステッピング・モータや、サーボ・モータを使う人が多いようだ。
しかし、あたしは、DCモータでやりたかった。
DCモータには、ノイズキラー・コンデンサを付ける。模型小僧の基本である。

DCモータ・ドライバの基板は、Raspi用に丁度いいものがあった。
https://www.sparkfun.com/products/11561
Sparkfun Electronics社のその名も「RaspiRobot Board, KIT-11561」!
「Raspiロボット・ボード」とは、よう言うたっ!!! (厚かましいぞwww)
スイッチサイエンスの通販で、￥3,294円。

このボードは、
DCモータ・ドライバIC L293DNEと、7406(オープン・コレクタ高耐圧バッファ)を使用し、
Raspiの GPIO に7406やL293DNEを接続しているだけのシンプルで
オーソドックスな基板となっている。

諸元としては、

双方向モータ制御×2
オープンコレクタ出力(7406)(25mA)×2
LED×2
スイッチ(デジタル)入力×2
5Vシリアル・コネクタ
3.3V I2Cコネクタ
Python用ライブラリ有り

このロボット・ボードは、設計としては、電源として、6～10V程度を入力すると、レギュレータで電圧を降圧したのち、
Raspi本体にも電源供給をするようになっている。
しかし、本機のように、USBからの流れ出しが多いシステムでは、このボードのオマケの軟弱なレギュレータでは、
電流が足りない。
したがって、現在、僕は、3端子レギュレータは使用してない。

そして、モータ用の電源と、ロジックの電源を分けている。ロジック電源の方には、電解コンデンサを増量している。

どうも、Sparkfun社のボードは、電源回りの考えが変だ。
以前も、Arduino 用の USBホスト・シールドも、電源の取り回しで、細工をせざるを得なかった。
(この方のやりかたを、真似たのだった。)
電源以外は、まぁ、満足できるので、いいのだが。

このボードを Raspi に装着すると、通常の Raspi のケースには入らない。
というか、割と嫌な感じではみ出す。まぁ、いいんだけど…

本システムの構成は、下図のとおり。

Raspi単体で、顔認識して、どう動くかを考え、モータも制御する。

認識結果の表示のためだけに、WiFi で、PCと通信し、
X protocol over ssh で、認識結果を PC上の X サーバに表示する。
結果表示が不要であれば、WiFiもPCも不要で、完全に単独で走行する。

ソフトウェアは、
人間の顔を見つけたら、
顔の位置が、認識対象の視野の中央に来るように、
右へ旋回/左へ旋回を決める。
顔の位置がセンターであれば、認識された顔の大きさをみて、
ちょうどいいサイズになるまで、前進/後進を行う。

カメラ入力の処理、顔認識、認識結果の表示は、すべてOpenCVの
機能を使用している。
極めて、簡単なソースコードでできている。

モータの制御は、ロボット・ボードのサンプルとして与えられた
プログラムを適当に変更している。
Raspiの GPIO のIOポートを、MMAPでユーザ空間に貼り付け、
メモリアクセスと同様に、IOポートをアクセスしている。

-- folow2.c


/* follow2
*/

#include "cv.h"
#include "highgui.h"

/*char* cascade = "/usr/share/opencv/haarcascades/haarcascade_frontalface_default.xml";*/
char* cascade = "/usr/local/opencv/share/OpenCV/haarcascades/haarcascade_frontalface_default.xml";

#define DISP_WIN "faceDetect"

void motor_drive(int cap_width,int last_face_x,int last_face_width);
extern void turn_right();
extern void turn_left();
extern void turn_stop();
extern void turn_stopNo();
extern void motor_gpio_init();
extern void go_forward();
extern void go_back();

CvSize minsiz ={0,0};

int last_face_x, last_face_width;
int cap_width=160;
int disp_and_sleepf=0;

int
main( int argc, char** argv )
{
  int i;
  double w = (double)cap_width;
  double h = 120;

  motor_gpio_init();

    cvNamedWindow( DISP_WIN , CV_WINDOW_AUTOSIZE );
    CvCapture* capture = NULL;
    if (argc > 1){
	disp_and_sleepf=1;
    }else{
    }
    capture = cvCreateCameraCapture( 0 );
    // (2)キャプチャサイズを設定する．
    cvSetCaptureProperty (capture, CV_CAP_PROP_FRAME_WIDTH, w);
    cvSetCaptureProperty (capture, CV_CAP_PROP_FRAME_HEIGHT, h);
    IplImage* frame;

    // 正面顔検出器の読み込み
    CvHaarClassifierCascade* cvHCC = (CvHaarClassifierCascade*)cvLoad(cascade, NULL,NULL,NULL);

    // 検出に必要なメモリストレージを用意する
    CvMemStorage* cvMStr = cvCreateMemStorage(0);
    // 検出情報を受け取るためのシーケンスを用意する
    CvSeq* face;
    while(1) {
        frame = cvQueryFrame( capture );
        if( !frame ) break;
        // 画像中から検出対象の情報を取得する

        face = cvHaarDetectObjects(frame, cvHCC, cvMStr,
				   1.2, 2, CV_HAAR_DO_CANNY_PRUNING,
				   minsiz, minsiz);
   //				   1.1, 3, 0, minsiz, minsiz);
	//////cvHaarDetectObjects( //const CvArr* image,

        for(i = 0; i < face->total; i++) {
            // 検出情報から顔の位置情報を取得
            CvRect* faceRect = (CvRect*)cvGetSeqElem(face, i);
            // 取得した顔の位置情報に基づき、矩形描画を行う
	    cvRectangle(frame,
                        cvPoint(faceRect->x, faceRect->y),
                        cvPoint(faceRect->x + faceRect->width,
				faceRect->y + faceRect->height),
                        CV_RGB(255, 0 ,0),
                        2, CV_AA, 0);
	    last_face_x=(int)faceRect->x;
	    last_face_width=(int)faceRect->width;
        }

	motor_drive(
         (face->total <=0)? -1: cap_width,
		  last_face_x, last_face_width);

	if(disp_and_sleepf){
		cvShowImage( DISP_WIN, frame );
		//char c = cvWaitKey(33);
		char c = cvWaitKey(1);
		if( c == 27 ) break;
	}
    }
    // 用意したメモリストレージを解放
    cvReleaseMemStorage(&cvMStr);
    // カスケード識別器の解放
    cvReleaseHaarClassifierCascade(&cvHCC);
    cvReleaseCapture( &capture );
    cvDestroyWindow( DISP_WIN );
}


#define HISTER 20
//#define HISTER 5
#define WANT_SIZE 40
#define SIZE_HISTER 5
//#define COUNT_SHIKII 30
#define COUNT_SHIKII 0

int turn_state =0;
int turn_count =0;

void
motor_drive(int cap_width,int last_face_x,int last_face_width)
{
  int center = cap_width/2;
  int face_x;
  int new_state;

//test0();

  face_x= last_face_x + (last_face_width/2);
printf(" size = %d \n", last_face_width);

  /* centering */
  if(cap_width <0){
      /* stop (no face)*/
    turn_state =0;
    turn_stopNo();
    return;
  }else if(face_x < (center - HISTER)){
      /* turn right (man view)*/
    new_state =1;
    turn_state =new_state;
    turn_right();
    return;
  }else if(face_x > (center + HISTER)){
      /* turn left (man view)*/
    new_state =2;
    turn_state =new_state;
    turn_left();
    return;
  }


 /* size */
 if(last_face_width<(WANT_SIZE -SIZE_HISTER)){
	go_forward();
	return;
 }
 if(last_face_width>(WANT_SIZE +SIZE_HISTER)){
	go_back();
	return;
 }

    /* stop (man center & good size)*/
    turn_state =0;
    turn_stop();
    return;

}

-- motor.c


//
//  How to access GPIO registers from C-code on the Raspberry-Pi
//  Example program
//  15-January-2012
//  Dom and Gert
//  Revised: 15-Feb-2013


// Access from ARM Running Linux

#define BCM2708_PERI_BASE        0x20000000
#define GPIO_BASE                (BCM2708_PERI_BASE + 0x200000) /* GPIO controller */


#include 
#include 
#include 
#include 
#include 

#define PAGE_SIZE (4*1024)
#define BLOCK_SIZE (4*1024)

int  mem_fd;
void *gpio_map;

// I/O access
volatile unsigned *gpio;


// GPIO setup macros. Always use INP_GPIO(x) before using OUT_GPIO(x) or SET_GPIO_ALT(x,y)
#define INP_GPIO(g) *(gpio+((g)/10)) &= ~(7<<(((g)%10)*3))
#define OUT_GPIO(g) *(gpio+((g)/10)) |=  (1<<(((g)%10)*3))
#define SET_GPIO_ALT(g,a) *(gpio+(((g)/10))) |= (((a)<=3?(a)+4:(a)==4?3:2)<<(((g)%10)*3))

#define GPIO_SET *(gpio+7)  // sets   bits which are 1 ignores bits which are 0
#define GPIO_CLR *(gpio+10) // clears bits which are 1 ignores bits which are 0

#define SET_OUT_MODE(g) { INP_GPIO(g); OUT_GPIO(g);}

void setup_io();
void turn_right();
void turn_left();
void turn_stop();
void motor_gpio_init();
void init_sign_led();
void led1(int);
void led2(int);


  // Set GPIO pins 7-11 to output

//int main(int argc, char **argv)
void
motor_gpio_init()
{
  int g;

  // Set up gpi pointer for direct register access
  setup_io();

  // Switch GPIO 7..11 to output mode

 /************************************************************************\
  * You are about to change the GPIO settings of your computer.          *
  * Mess this up and it will stop working!                               *
  * It might be a good idea to 'sync' before running this program        *
  * so at least you still have your code changes written to the SD-card! *
 \************************************************************************/

  // Set GPIO pins 7,8,21,22, 17,4,25 to output
	SET_OUT_MODE(7); // LED1
	SET_OUT_MODE(8); // LED2
	SET_OUT_MODE(21); //OC output
	SET_OUT_MODE(22);
	SET_OUT_MODE(17); //Motor
	SET_OUT_MODE(4);
	SET_OUT_MODE(10);
	SET_OUT_MODE(25);

       GPIO_CLR = 1<<(17); // 1-2EN OFF
       GPIO_CLR = 1<<(10); // 3-4EN OFF

       GPIO_CLR = 1<<(4); // 1A OFF
       GPIO_CLR = 1<<(25); // 3A OFF

	init_sign_led();

       return ;
}

void
init_sign_led()
{int i;
 for(i=0;i<3;i++){
	led1(1);led2(1);
	sleep(1);
	led1(0);led2(0);
	sleep(1);
 }
}

void
test0()
{
  int g,rep;
     GPIO_CLR = 1<<(10); // 3-4EN OFF
     GPIO_SET = 1<<(17); // 1-2EN ON

     GPIO_SET = 1<<7; //LED1
     GPIO_SET = 1<<(4); // 1A ON
     sleep(3);

     GPIO_CLR = 1<<7; //LED1 OFF
     GPIO_CLR = 1<<(4); // 1A OFF
     sleep(3);
     /*--*/
     GPIO_CLR = 1<<(17); // 1-2EN OFF
     GPIO_SET = 1<<(10); // 3-4EN ON

     GPIO_SET = 1<<8; //LED2
     GPIO_SET = 1<<(25); // 3A ON
     sleep(3);

     GPIO_CLR = 1<<8; //LED2 OFF
     GPIO_CLR = 1<<(25); // 3A OFF
     sleep(3);

}

void
test1()
{
  int g,rep;
  for(rep=0; rep<10; rep++) {
	test0();
  }
  return ;
}

#define MOTOR_ON_TIME 30000


void
go_forward()
{
  printf("forward()\n");
     GPIO_SET = 1<<7; //LED1
     GPIO_SET = 1<<8; //LED2
     GPIO_CLR = 1<<(17); // 1-2EN OFF
     GPIO_CLR = 1<<(10); // 3-4EN OFF
     GPIO_CLR = 1<<(4); // 1A OFF
     GPIO_CLR = 1<<(25); // 3A OFF
     GPIO_SET = 1<<(17); // 1-2EN ON
     GPIO_SET = 1<<(10); // 3-4EN ON
     usleep(MOTOR_ON_TIME);
     GPIO_CLR = 1<<(17); // 1-2EN OFF
     GPIO_CLR = 1<<(10); // 3-4EN OFF
}

void
go_back()
{
  printf("back()\n");
     GPIO_SET = 1<<7; //LED1
     GPIO_SET = 1<<8; //LED2
     GPIO_CLR = 1<<(17); // 1-2EN OFF
     GPIO_CLR = 1<<(10); // 3-4EN OFF
     GPIO_SET = 1<<(4); // 1A ON
     GPIO_SET = 1<<(25); // 3A ON
     GPIO_SET = 1<<(17); // 1-2EN ON
     GPIO_SET = 1<<(10); // 3-4EN ON
     usleep(MOTOR_ON_TIME);
     GPIO_CLR = 1<<(17); // 1-2EN OFF
     GPIO_CLR = 1<<(10); // 3-4EN OFF
}

void
turn_right()
{
  printf("turn_right()\n");
     GPIO_SET = 1<<7; //LED1
     GPIO_CLR = 1<<8; //LED2
     GPIO_CLR = 1<<(17); // 1-2EN OFF
     GPIO_CLR = 1<<(10); // 3-4EN OFF
     GPIO_SET = 1<<(4); // 1A ON
     GPIO_CLR = 1<<(25); // 3A OFF
     GPIO_SET = 1<<(17); // 1-2EN ON
     GPIO_SET = 1<<(10); // 3-4EN ON
     usleep(MOTOR_ON_TIME);
     GPIO_CLR = 1<<(17); // 1-2EN OFF
     GPIO_CLR = 1<<(10); // 3-4EN OFF
}

void
turn_left()
{
  printf("turn_left()\n");
     GPIO_CLR = 1<<7; //LED1
     GPIO_SET = 1<<8; //LED2
     GPIO_CLR = 1<<(17); // 1-2EN OFF
     GPIO_CLR = 1<<(10); // 3-4EN OFF
     GPIO_CLR = 1<<(4); // 1A OFF
     GPIO_SET = 1<<(25); // 3A ON
     GPIO_SET = 1<<(17); // 1-2EN ON
     GPIO_SET = 1<<(10); // 3-4EN ON
     usleep(MOTOR_ON_TIME);
     GPIO_CLR = 1<<(17); // 1-2EN OFF
     GPIO_CLR = 1<<(10); // 3-4EN OFF
}

void
turn_stop()
{
  printf("turn_stop()\n");
     GPIO_SET = 1<<7; //LED1
     GPIO_SET = 1<<8; //LED2
     GPIO_CLR = 1<<(17); // 1-2EN OFF
     GPIO_CLR = 1<<(10); // 3-4EN OFF
}

void
turn_stopNo()
{
  printf("turn_stop()\n");
     GPIO_CLR = 1<<7; //LED1
     GPIO_CLR = 1<<8; //LED2
     GPIO_CLR = 1<<(17); // 1-2EN OFF
     GPIO_CLR = 1<<(10); // 3-4EN OFF
}

void
led1(int x)
{
 if(x){
     GPIO_SET = 1 <<7; //LED1
 }else{
     GPIO_CLR = 1 <<7; //LED1
 }
}

void
led2(int x)
{
 if(x){
     GPIO_SET = 1 <<8; //LED2
 }else{
     GPIO_CLR = 1 <<8; //LED2
 }
}



//
// Set up a memory regions to access GPIO
//
void setup_io()
{
   /* open /dev/mem */
   if ((mem_fd = open("/dev/mem", O_RDWR|O_SYNC) ) < 0) {
      printf("can't open /dev/mem \n");
      exit(-1);
   }

   /* mmap GPIO */
   gpio_map = mmap(
      NULL,             //Any adddress in our space will do
      BLOCK_SIZE,       //Map length
      PROT_READ|PROT_WRITE,// Enable reading & writting to mapped memory
      MAP_SHARED,       //Shared with other processes
      mem_fd,           //File to map
      GPIO_BASE         //Offset to GPIO peripheral
   );

   close(mem_fd); //No need to keep mem_fd open after mmap

   if (gpio_map == MAP_FAILED) {
      printf("mmap error %d\n", (int)gpio_map);//errno also set!
      exit(-1);
   }

   // Always use volatile pointer!
   gpio = (volatile unsigned *)gpio_map;


} // setup_io

-- Makefile


ALL= follow follow2

CXX = g++
# package openCV
LDFLAGS = -lopencv_legacy -lopencv_highgui -lopencv_core -lopencv_ml -lopencv_video -lopencv_imgproc -lopencv_calib3d -lopencv_objdetect -L/usr/lib
CPPFLAGS = -mfpu=vfpv3 -O4 -I/usr/include/opencv -I/usr/include/opencv2 
##CPPFLAGS = -g -I/usr/include/opencv -I/usr/include/opencv2 

all: $(ALL)

follow2 : follow2.o motor.o
	$(CXX) -o $@  $< $(LDFLAGS)  motor.o

follow2.o : follow2.c
	$(CXX) -c  $< $(CPPFLAGS)

follow : follow.o motor.o
	$(CXX) -o $@  $< $(LDFLAGS)  motor.o

follow.o : follow.c
	$(CXX) -c  $< $(CPPFLAGS)

motor.o : motor.c
	$(CXX) -c  $< $(CPPFLAGS)

clean:  
	rm -f $(ALL) *.o

たけおかぼちぼち日記

思いついたらメモ

Raspberry Piで、OpenCV タンク(自走式)を作る