FC2カウンター FPGAの部屋 Zybot で Gabor filter を使うためのZYBO_0_5 プロジェクト5(走行テスト)

FPGAやCPLDの話題やFPGA用のツールの話題などです。 マニアックです。 日記も書きます。

FPGAの部屋

FPGAの部屋の有用と思われるコンテンツのまとめサイトを作りました。Xilinx ISEの初心者の方には、FPGAリテラシーおよびチュートリアルのページをお勧めいたします。

Zybot で Gabor filter を使うためのZYBO_0_5 プロジェクト5(走行テスト)

Zybot で Gabor filter を使うためのZYBO_0_5 プロジェクト4(実機確認)”の続き。

今日はZybot で白線追従走行用ソフトウェアの wl_tracking_dmaw.c をコンパイルして、Zybot を走らせてみた。
前よりは追従性が上がったようだが、カーブ手前で曲がり始めてしまう気がした。反応が速くなったのは良いのだが、白線追従走行用ソフトウェアのチューニングは必要だ。
それとすぐに白線追従走行ができなくなって、どこかに行ってしまう時もある。それは、最初に真っ直ぐにコースの置かなかった時のようだ。カメラのレンズが望遠すぎるのも問題な気がする。広角用のレンズを探すか、もしくは、携帯用の広角アダプタが付けられないか?調べてみよう。
だが、今度は、左と右の白線検出用のガボール・フィルタをハードウェアで分けたので、判定制御間隔は確実に短くなっている。前の走行よりも細かく制御できているのが、よくわかった。
走行テストのうまく行った様子を示す。


wl_tracing_damw.c を貼っておく。

// wl_tracking_dmaw.cpp
// 2016/09/28 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

// Gobor filter
//
void gabor_fil_on(){
    int fd2, fd3, fd4;
    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[0] = 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);
    munmap((void *)gabor_filter_lh_0, 0x10000);
    
    close(fd2);
    close(fd3);
    close(fd4);   
}

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);
}

// DMAW4Gabor setting
volatile unsigned *dmaw4g_initialize(int &fd1){
    volatile unsigned *dmaw4gabor_0;

    fd1 = open("/dev/uio1", O_RDWR); // dmaw4gabor_0 interface AXI4 Lite Slave
    if (fd1 < 1){
        fprintf(stderr, "/dev/uio1 (dmaw4gabor_0) open error\n");
        exit(-1);
    }
    dmaw4gabor_0 = (volatile unsigned *)mmap(NULL, 0x10000, PROT_READ|PROT_WRITE, MAP_SHARED, fd1, 0);
    if (!dmaw4gabor_0){
        fprintf(stderr, "dmaw4gabor_0 mmap error\n");
        exit(-1);
    }

    return(dmaw4gabor_0);
}


int main(){
    int fd1;
    volatile unsigned *dmaw4gabor_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_fil_on();

    // Motor Initialize
    motor_initialize(motorL, motorR, mmL, mmR);
    
    // DMAW4Gabor Initialize
    dmaw4gabor_0 = dmaw4g_initialize(fd1);

    // 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);
    
    // DMA4Gabor frame_buffer1 setting
    dmaw4gabor_0[8] = (int)phys_addr + SVGA_ALL_DISP_ADDRESS; // Data signal of frame_buffer1

    // main loop
    printf("White line Tracking start. \n");
    while(1){
        // Gabor filter for left white line
        left_wl_edge = SVGA_HORIZONTAL_PIXELS/2 - search_gabor_edge(
            (unsigned int)frame_buffer+GABOR_DETECT_LINE_ADDR, SVGA_HORIZONTAL_PIXELS/2, GABOR_THRESHOLD);

        // Gabor filter for right white line
        right_wl_edge = search_gabor_edge(
            (unsigned int)frame_buffer+SVGA_ALL_DISP_ADDRESS+GABOR_DETECT_LINE_ADDR+(SVGA_HORIZONTAL_PIXELS/2)*PIXEL_NUM_OF_BYTES,
            SVGA_HORIZONTAL_PIXELS/2, GABOR_THRESHOLD);

#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
        }
    }
}

  1. 2016年10月05日 04:50 |
  2. Zybot
  3. | トラックバック:0
  4. | コメント:0

コメント

コメントの投稿


管理者にだけ表示を許可する

トラックバック URL
http://marsee101.blog19.fc2.com/tb.php/3592-07c5f45c
この記事にトラックバックする(FC2ブログユーザー)