Zybotのカメラによる白線追従走行

Zybot の白線検出、追従走行のアプリケーション・ソフトウェアを作って、白線追従走行させてみました。
その様子です。




まだまだ、ON-OFF 制御で走行もふらつき気味ですが、一応、白線追従走行はできるようになりました。
ただ、直角に曲がるのは難しいですね。後ろの車輪をサーボモーターで向きを変えるか？前輪を左右逆転するとかしないといけないのでしょうか？
もっと車輪の間隔を狭めた方が小回り効きそうです。トレッドを短くするということですね。
ロボットカーは画像処理の要素だけでなく、車の要素が入ってくるので、制御が難しいですね。。。
なお、現在のZybot は、後車輪の自在キャスターが回りすぎてしまうので、クリップを改造して回り止めを付けています。


現在の制御用のアプリケーション・ソフトウェアを貼っておきます。

// wl_tracking.cpp
// 2016/09/02 by marsee
//

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

#include "xpwm.h"
#include "xmotor_monitor.h"

#define DIR_LEFT_NORMAL        1
#define DIR_LEFT_REVERSE    0
#define DIR_RIGHT_NORMAL    0
#define DIR_RIGHT_REVERSE    1

#define PIXEL_NUM_OF_BYTES    4
#define SVGA_HORIZONTAL_PIXELS  800
#define SVGA_VERTICAL_LINES     600
#define SVGA_ALL_DISP_ADDRESS   (SVGA_HORIZONTAL_PIXELS * SVGA_VERTICAL_LINES * PIXEL_NUM_OF_BYTES)

#define GABOR_DETECT_LINE        590
#define GABOR_DETECT_LINE_ADDR    (SVGA_HORIZONTAL_PIXELS * GABOR_DETECT_LINE * PIXEL_NUM_OF_BYTES)
#define GABOR_THRESHOLD            5
#define DIST_THRESHOLD            30

#define LEFT_GABOR_EDGE_OVERFLOW    0
#define RIGHT_GABOR_EDGE_OVERFLOW    (SVGA_HORIZONTAL_PIXELS/2)

#define DEBUG
//#define MOTOR_OFF
#define MEMCPY

// Gobor filter
//
volatile unsigned *gabor_fil_on(int &fd4){
    int fd2, fd3;
    volatile unsigned *axis_switch_0, *axis_switch_1;
    volatile unsigned *gabor_filter_lh_0;
    int gabor_cntrl;

   // axis_switch_0 (UIO2)
    fd2 = open("/dev/uio2", O_RDWR); // axis_switch_0 interface AXI4 Lite Slave
    if (fd2 < 1){
        fprintf(stderr, "/dev/uio2 (axis_switch_0) open error\n");
        exit(-1);
    }
    axis_switch_0 = (volatile unsigned *)mmap(NULL, 0x10000, PROT_READ|PROT_WRITE, MAP_SHARED, fd2, 0);
    if (!axis_switch_0){
        fprintf(stderr, "axis_switch_0 mmap error\n");
        exit(-1);
    }
    
    // axis_switch_1 (UIO3)
    fd3 = open("/dev/uio3", O_RDWR); // axis_switch_1 interface AXI4 Lite Slave
    if (fd3 < 1){
        fprintf(stderr, "/dev/uio3 (axis_switch_1) open error\n");
        exit(-1);
    }
    axis_switch_1 = (volatile unsigned *)mmap(NULL, 0x10000, PROT_READ|PROT_WRITE, MAP_SHARED, fd3, 0);
    if (!axis_switch_1){
        fprintf(stderr, "axis_switch_1 mmap error\n");
        exit(-1);
    }
    
    // gabor_filter_lh_0 (UIo14)
    fd4 = open("/dev/uio14", O_RDWR); // gabor_filter_lh_0 interface AXI4 Lite Slave
    if (fd4 < 1){
        fprintf(stderr, "/dev/uio14 (gabor_filter_lh_0) open error\n");
        exit(-1);
    }
    gabor_filter_lh_0 = (volatile unsigned *)mmap(NULL, 0x10000, PROT_READ|PROT_WRITE, MAP_SHARED, fd4, 0);
    if (!gabor_filter_lh_0){
        fprintf(stderr, "lap_filter_axis_0 mmap error\n");
        exit(-1);
    }
      
    // axis_switch_1, 1to2 ,Select M01_AXIS
    // Refer to http://marsee101.blog19.fc2.com/blog-entry-3177.html
    axis_switch_1[16] = 0x80000000; // 0x40 = 0x80000000; disable
    axis_switch_1[17] = 0x80000000; // 0x44 = 0x80000000; disable
    axis_switch_1[18] = 0; // 0x48 = 0;
    axis_switch_1[0] = 0x2; // 0x0 = 2; Commit registers
    
    // gabor filter AXIS Start
    gabor_filter_lh_0[6] = 0; // left parameter
    gabor_cntrl = gabor_filter_lh_0[0] & 0x80; // Auto Restart bit
    gabor_filter_lh_0[0] = gabor_cntrl | 0x01; // Start bit set
    gabor_filter_lh_0[0] = 0x80; // Auto Restart bit set
    
    // axis_switch_0, 2to1, Select S01_AXIS
    // Refer to http://marsee101.blog19.fc2.com/blog-entry-3177.html
    axis_switch_0[16] = 0x2; // 0x40 = 0x2;
    axis_switch_0[0] = 0x2; // 0x0 = 2; Commit registers

    munmap((void *)axis_switch_0, 0x10000);
    munmap((void *)axis_switch_1, 0x10000);
    
    close(fd2);
    close(fd3);
    
    return(gabor_filter_lh_0);
}

int search_gabor_edge(unsigned int start_addr, unsigned int number, int threshold){
    volatile int *imgaddr, *endaddr;
    int i;
    
    imgaddr = (volatile int *)start_addr;
    
    for (i=0; i<number; i++){
        int c=imgaddr[i] & 0xff;
        //printf("%d\n",c);
        if (c >= threshold){
            break;
        }
    }
    return(i); 
}

// Motor
//
void motor_settings(XPwm *motorLp, XPwm *motorRp){
    XPwm_DisableAutoRestart(motorLp);
    while(!XPwm_IsIdle(motorLp)) ;
    XPwm_Start(motorLp);
    XPwm_EnableAutoRestart(motorLp);
    
     XPwm_DisableAutoRestart(motorRp);
    while(!XPwm_IsIdle(motorRp)) ;
    XPwm_Start(motorRp);
    XPwm_EnableAutoRestart(motorRp);
}

void Stopped_Zybot(XPwm *motorLp, XPwm *motorRp){
    XPwm_Set_sw_late_V(motorLp, 0);
    XPwm_Set_sw_late_V(motorRp, 0);
}

void motor_initialize(XPwm &motorL, XPwm &motorR, XMotor_monitor &mmL, XMotor_monitor &mmR){
    XPwm *motorLp, *motorRp;
    XMotor_monitor *mmLp, *mmRp;
    
    motorLp = &motorL;
    motorRp = &motorR;
    mmLp = &mmL;
    mmRp = &mmR;
    
    // Initialization of motor
    if (XPwm_Initialize(motorLp, "pwm_0") != XST_SUCCESS){
        fprintf(stderr,"pwm_0 (Left) open error\n");
        exit(-1);
    }
    if (XPwm_Initialize(motorRp, "pwm_1") != XST_SUCCESS){
        fprintf(stderr,"pwm_1 (Right) open error\n");
        exit(-1);
    }
    
    
    // Initialization of motor monitor
    if (XMotor_monitor_Initialize(mmLp, "motor_monitor_0") != XST_SUCCESS){
        fprintf(stderr,"motor_monitor_0 (Left) open error\n");
        exit(-1);
    }
    if (XMotor_monitor_Initialize(mmRp, "motor_monitor_1") != XST_SUCCESS){
        fprintf(stderr,"motor_monitor_1 (Right) open error\n");
        exit(-1);
    }

    // The Motors is rotated in the forward direction.
    XPwm_Set_sw_late_V(motorLp, 0);
    XPwm_Set_dir_V(motorLp, 1);

    XPwm_Set_sw_late_V(motorRp, 0);
     XPwm_Set_dir_V(motorRp, 0);

    motor_settings(motorLp, motorRp);
}


int main(){
    int fd4;
    volatile unsigned *gabor_filter_lh_0;
    XPwm motorL, motorR;
    XMotor_monitor mmL, mmR;
    unsigned char  attr[1024];
    unsigned long  phys_addr;
      int left_wl_edge, right_wl_edge;

    // Gabor filter Initialize
    gabor_filter_lh_0 = gabor_fil_on(fd4);

    // Motor Initialize
    motor_initialize(motorL, motorR, mmL, mmR);
    
    // udmabuf0
    int fdf = open("/dev/udmabuf0", O_RDWR | O_SYNC); // frame_buffer, The cache is disabled. 
    if (fdf == -1){
        fprintf(stderr, "/dev/udmabuf0 open error\n");
        exit(-1);
    }
    volatile unsigned *frame_buffer = (volatile unsigned *)mmap(NULL, 3*SVGA_ALL_DISP_ADDRESS, PROT_READ|PROT_WRITE, MAP_SHARED, fdf, 0);
    if (!frame_buffer){
        fprintf(stderr, "frame_buffer mmap error\n");
        exit(-1);
    }

    // phys_addr of udmabuf0
    int fdp = open("/sys/devices/virtual/udmabuf/udmabuf0/phys_addr", O_RDONLY);
    if (fdp == -1){
        fprintf(stderr, "/sys/devices/virtual/udmabuf/udmabuf0/phys_addr open error\n");
        exit(-1);
    }
    read(fdp, attr, 1024);
    sscanf((const char *)attr, "%lx", &phys_addr);  
    close(fdp);
    printf("phys_addr = %x\n", (unsigned int)phys_addr);
    
    // main loop
    printf("White line Tracking start. \n");
    while(1){
        // Gabor filter for left white line 
        gabor_filter_lh_0[6] = 0; // left parameter

        usleep(250000); // Wait 67 ms
        left_wl_edge = SVGA_HORIZONTAL_PIXELS/2 - search_gabor_edge(
            (unsigned int)frame_buffer+GABOR_DETECT_LINE_ADDR, SVGA_HORIZONTAL_PIXELS/2, GABOR_THRESHOLD);
#ifdef MEMCPY
        memcpy((unsigned int *)((unsigned int)frame_buffer+SVGA_ALL_DISP_ADDRESS), (unsigned int *)frame_buffer,
            SVGA_ALL_DISP_ADDRESS);
#endif

        // Gabor filter for right white line
        gabor_filter_lh_0[6] = 1; // right parameter
        usleep(250000); // Wait 67 ms
        right_wl_edge = search_gabor_edge(
            (unsigned int)frame_buffer+GABOR_DETECT_LINE_ADDR+(SVGA_HORIZONTAL_PIXELS/2)*PIXEL_NUM_OF_BYTES,
            SVGA_HORIZONTAL_PIXELS/2, GABOR_THRESHOLD);
#ifdef MEMCPY
        memcpy((unsigned int *)((unsigned int)frame_buffer+2*SVGA_ALL_DISP_ADDRESS), (unsigned int *)frame_buffer,
            SVGA_ALL_DISP_ADDRESS);
#endif

#ifdef DEBUG
        printf("left_wl_edge = %d, right_wl_edge = %d\n", left_wl_edge, right_wl_edge);
#endif

        if (left_wl_edge == LEFT_GABOR_EDGE_OVERFLOW){
#ifndef MOTOR_OFF
            XPwm_Set_sw_late_V(&motorL, 5);
            XPwm_Set_sw_late_V(&motorR, 35);
#endif
#ifdef DEBUG
            printf("Left gabor edge is overflow\n");
#endif
        } else if (right_wl_edge == RIGHT_GABOR_EDGE_OVERFLOW){
#ifndef MOTOR_OFF
            XPwm_Set_sw_late_V(&motorL, 35);
            XPwm_Set_sw_late_V(&motorR, 5);
#endif
#ifdef DEBUG
            printf("Right gabar edge is overflow\n");
#endif
        } else if ((right_wl_edge - left_wl_edge) > DIST_THRESHOLD){
#ifndef MOTOR_OFF
            XPwm_Set_sw_late_V(&motorL, 25);
            XPwm_Set_sw_late_V(&motorR, 15);
#endif
#ifdef DEBUG
            printf("Right turn\n");
#endif
        } else if ((right_wl_edge - left_wl_edge) < -DIST_THRESHOLD){
#ifndef MOTOR_OFF
            XPwm_Set_sw_late_V(&motorL, 15);
            XPwm_Set_sw_late_V(&motorR, 25);
#endif
#ifdef DEBUG
            printf("Left turn\n");
#endif
        } else if (abs(right_wl_edge - left_wl_edge) <= DIST_THRESHOLD){
#ifndef MOTOR_OFF
            XPwm_Set_sw_late_V(&motorL, 20);
            XPwm_Set_sw_late_V(&motorR, 20);
#endif
#ifdef DEBUG
            printf("Go straight\n");
#endif
        }
    }
}

なぜか、ガボール・フィルタを設定してから、67 ms 待つだけでは画像が完全に処理しきれていないことがあったので、待ち時間を延ばしました。なんででしょうか？
左右のガボール・フィルタ画像を別のフレームバッファに割り当ててDMA転送すれば、もっと速くできそうです。
カメラのfps を上げることも考えてみようと思います。