FC2カウンター FPGAの部屋 2台の Zybot での隊列走行5(追従走行を試してみた)

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

FPGAの部屋

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

2台の Zybot での隊列走行5(追従走行を試してみた)

2台の Zybot での隊列走行4(追従走行用アプリケーションの作成1)”の続き。

前回は、追従走行用アプリケーション・ソフトウェアを作成し、認識テストを行った。今回は、実際に2台の Zybot を使用して追従走行を試してみた。

隊列走行の写真を示す。
platoon_36_161201.jpg

なお、追従するZybot のカメラを割りばしで固定した。こうしないとカメラがコネクタでしか固定されていないので、首を振ってしまって、マーカーを良い位置で捉えられない。そうすると、追従走行が不安定になってしまうので、固定する必要があった。
platoon_37_161201.jpg

追従走行 Zybot 2 台を後ろから見た写真だ。
platoon_38_161201.jpg

追従走行している様子。
platoon_39_161201.jpg

やはり、カメラが望遠気味で、カメラの画角から前走車が出てしまったり、前走車が急に曲がられるとマーカーが見えなくなって、追従走行できなかったりした。マーカーを円柱にできれば良いのだろうが、素早くどういう状況になっているかを判断する必要があるようだ。
検出方法は画像のある1行をサーチして、15ピクセル以上水色があって、赤色が何ピクセル続いたかを計測し、その後水色が出てくるところまで検索している。これは、前走車の速さが速いと画角から外れて追従車が止まってしまうので、その場合はある一定幅の赤が画面のどちらにあるか?を検索して、急いで曲がった方が良いと思う。
後、やはり、カメラのフレームレートが遅いのか?アプリケーションの実行時間が長いのか?前走車に追従車が遅れてしまうことがあるようだ。これを何とかしないと。。。

YouTube に追従走行の様子をアップロードしたので、ブログに貼っておく。制御がPID制御になっていないので、多少ぎごちない。




現在の追従走行用アプリケーション platoon_car1.cpp を貼っておく。ノンブロッキングのキー入力ができた。

// platoon_car1.cpp
// Autonomously running car
// 2016/11/22 by marsee
// 
// Reffered to http://d.hatena.ne.jp/mFumi/20101002/1286003738
//

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <assert.h>
#include <sys/mman.h>
#include <fcntl.h>
#include <string.h>
#include <termios.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 H_180_HIGH_LIMIT    210
#define H_180_LOW_LIMIT        150
#define H_0_HIGH_LIMIT        30
#define H_0_LOW_LIMIT        330

#define CHECK_LINE_NUMBER    290

#define RED_LOW_WIDTH        280 // red size
#define RED_HIGH_WIDTH        340 // red size
#define RED_FAR_WIDTH        200
#define RED_OUT_OF_RANGE    30 // red size

#define RED_MORE_LEFT_POS    225
#define RED_CENTER_POS_LOW    350
#define RED_CENTER_POS_HIGH    450
#define RED_MORE_RIGHT_POS    575

#define DETECTED_A_MAEKER    15    // When the light blue judgment is continued for about 15 pixels, it is judged as a marker.

//#define DEBUG
//#define MOTOR_OFF

// Start RGB2HSV 
//
void rgb2hsv_on(){
    int fd2, fd3, fd15;
    volatile unsigned *axis_switch_0, *axis_switch_1;
    volatile unsigned *rgb2hsv_0;

   // 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);
    }
    
    // rgb2hsv_0 (UIO15)
    fd15 = open("/dev/uio15", O_RDWR); // rgb2hsv_0 interface AXI4 Lite Slave
    if (fd15 < 1){
        fprintf(stderr, "/dev/uio15 (rgb2hsv_0) open error\n");
        exit(-1);
    }
    rgb2hsv_0 = (volatile unsigned *)mmap(NULL, 0x10000, PROT_READ|PROT_WRITE, MAP_SHARED, fd15, 0);
    if (!rgb2hsv_0){
        fprintf(stderr, "rgb2hsv_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] = 0x80000000// 0x48 = 0x80000000, disable
    axis_switch_1[19] = 0x0// 0x4C = 0
    axis_switch_1[0] = 0x2// 0x0 = 2; Commit registers
    
    // rgb2hsv IP AXIS Start
    rgb2hsv_0[0] = 0x01// Start bit set
    rgb2hsv_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] = 0x3// 0x40 = 0x3;
    axis_switch_0[0] = 0x2// 0x0 = 2; Commit registers

    munmap((void *)axis_switch_0, 0x10000);
    munmap((void *)axis_switch_1, 0x10000);
    munmap((void *)rgb2hsv_0, 0x10000);
    
    close(fd2);
    close(fd3);
    close(fd15);   
}    

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

void SetMotorLR(XPwm &motorL, XPwm &motorR, int left_rate, int right_rate){
    static int PowerR=0;
    static int PowerL=0;
    
#ifndef MOTOR_OFF
    if (PowerR == 0 && PowerL == 0 && left_rate != 0 && right_rate != 0){
        XPwm_Set_sw_late_V(&motorL, 25);
        XPwm_Set_sw_late_V(&motorR, 25);
        usleep(500000);
    }
    XPwm_Set_sw_late_V(&motorL, left_rate);
    XPwm_Set_sw_late_V(&motorR, right_rate);
#endif
    PowerL = left_rate;
    PowerR = right_rate;
}

// Search Marker
// return 0 -- Success, return 1 -- I can not find a marker.
// 
int SearchMarker(volatile unsigned int *addr, int &red_size, int &red_start){
    enum STATE {IDLE, FIND_LIGHT_BLUE, FIND_RED, LAST_LIGHT_BLUE};
    enum STATE cs = IDLE;
    int light_blue = 0;
    int h;

    red_size = 0;

    for (int i=0; i<SVGA_HORIZONTAL_PIXELS; i++){
        h = (addr[i]/65536) & 0x1ff;
        switch (cs){
            case IDLE :
                if (light_blue<15){
                    if (h>H_180_LOW_LIMIT && h<H_180_HIGH_LIMIT){ // light_blue
                        light_blue++;
                    } else {
                        light_blue = 0;
                    }
                } else {
                    cs = FIND_LIGHT_BLUE;
                }
                break;
            case FIND_LIGHT_BLUE :
                if ((h>H_0_LOW_LIMIT && h<=360) || (h>=0 && h<H_0_HIGH_LIMIT)){ // red
                    if (red_size == 0)
                        red_start = i;
                    red_size++;
                    cs = FIND_RED;
                }
                break;
            case FIND_RED :
                if ((h>H_0_LOW_LIMIT && h<=360) || (h>=0 && h<H_0_HIGH_LIMIT)){ // red
                    red_size++;
                } else if (h>H_180_LOW_LIMIT && h<H_180_HIGH_LIMIT){ // light_blue
                    return(0); // end
                }
                break;
        }
    }
        
    return(1); // error
}

// debug_print()
void debug_print(const char s[]){
#ifdef DEBUG
    printf("%s\n", s);
    usleep(500000);
#endif
}


int main(){
    int fd1;
    XPwm motorL, motorR;
    XMotor_monitor mmL, mmR;
    unsigned char  attr[1024];
    unsigned long  phys_addr;
    int c;
    int red_size, red_start, red_center;
    struct termios save_settings;
    struct termios settings;

    // Reffered to http://d.hatena.ne.jp/mFumi/20101002/1286003738
    tcgetattr(0,&save_settings);
    settings = save_settings;

    settings.c_lflag &= ~(ECHO|ICANON);
    settings.c_cc[VTIME] = 0;
    settings.c_cc[VMIN] = 1;
    tcsetattr(0,TCSANOW,&settings);
    fcntl(0,F_SETFL,O_NONBLOCK);

    // Start RGB2HSV
    rgb2hsv_on();
    
    // 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 int *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("Start platoon.\n");
    printf("if pushed 'q' key then exit.\n");
    while(1){
        if ((c = getchar()) != EOF){
            if (c == 'q')
                break;
        }

        int result = SearchMarker(&frame_buffer[SVGA_HORIZONTAL_PIXELS*CHECK_LINE_NUMBER], red_size, red_start);
        red_center = red_start + red_size/2;
#ifdef DEBUG
        printf("red_size = %d, red_start = %d, red_center = %d\n", red_size, red_start, red_center);
#endif
        if (result){
            debug_print("error: can not find a marker.\n");
            SetMotorLR(motorL, motorR, 00); // stop
        } else {
            if (red_size < RED_OUT_OF_RANGE){ // too small
                debug_print("error: A maker is too small.\n");
                SetMotorLR(motorL, motorR, 00); //stop
            } else if (RED_LOW_WIDTH < red_size && red_size < RED_HIGH_WIDTH){ // Normal size
                if (RED_CENTER_POS_LOW < red_center && red_center < RED_CENTER_POS_HIGH){
                    debug_print("Normal size, Center\n");
                    SetMotorLR(motorL, motorR, 2525); // Slow Straight
                } else if (RED_MORE_LEFT_POS < red_center && red_center <= RED_CENTER_POS_LOW){ // Left side
                    debug_print("Normal size, Left side\n");
                    SetMotorLR(motorL, motorR, 1530); // Left turn
                } else if (red_center <= RED_MORE_LEFT_POS){ // More left side
                    debug_print("Normal size, More left side\n");
                    SetMotorLR(motorL, motorR, 1535); // Strong left turn
                } else if (RED_CENTER_POS_HIGH <= red_center && red_center < RED_MORE_RIGHT_POS){ // Right side
                    debug_print("Normal size, Right side\n");
                    SetMotorLR(motorL, motorR, 3015); // Right turn
                } else { // else if (RED_MORE_RIGHT_POS <= red_center){
                    debug_print("Normal size, More right side\n");
                    SetMotorLR(motorL, motorR, 3515); // Strong right turn
                }
            } else if (RED_FAR_WIDTH < red_size && red_size <= RED_LOW_WIDTH){ // Small red maker
                if (RED_CENTER_POS_LOW < red_center && red_center < RED_CENTER_POS_HIGH){
                    debug_print("Small size, Center\n");
                    SetMotorLR(motorL, motorR, 3030); // Straight
                } else if (RED_MORE_LEFT_POS < red_center && red_center <= RED_CENTER_POS_LOW){ // Left side
                    debug_print("Small size, Left side\n");
                    SetMotorLR(motorL, motorR, 2035); // Left turn
                } else if (red_center <= RED_MORE_LEFT_POS){ // More left side
                    debug_print("Small size, More left side\n");
                    SetMotorLR(motorL, motorR, 2040); // Strong left turn
                } else if (RED_CENTER_POS_HIGH <= red_center && red_center < RED_MORE_RIGHT_POS){ // Right side
                    debug_print("Small size, Right side\n");
                    SetMotorLR(motorL, motorR, 3520); // Right turn
                } else { // else if (RED_MORE_RIGHT_POS <= red_center){
                    debug_print("Small size, More right side\n");
                    SetMotorLR(motorL, motorR, 4020); // Strong right turn
                }
            }  else if (RED_OUT_OF_RANGE < red_size && red_size <= RED_FAR_WIDTH){ // More small red maker
                if (RED_CENTER_POS_LOW < red_center && red_center < RED_CENTER_POS_HIGH){
                    debug_print("More small size, Center\n");
                    SetMotorLR(motorL, motorR, 3535); // Straight fast
                } else if (RED_MORE_LEFT_POS < red_center && red_center <= RED_CENTER_POS_LOW){ // Left side
                    debug_print("More small size, Left side\n");
                    SetMotorLR(motorL, motorR, 2540); // Left turn
                } else if (red_center <= RED_MORE_LEFT_POS){ // More left side
                    debug_print("More small size, More left side\n");
                    SetMotorLR(motorL, motorR, 2545); // Strong left turn
                } else if (RED_CENTER_POS_HIGH <= red_center && red_center < RED_MORE_RIGHT_POS){ // Right side
                    debug_print("More small size, right side\n");
                    SetMotorLR(motorL, motorR, 4025); // Right turn
                } else { // else if (RED_MORE_RIGHT_POS <= red_center){
                    debug_print("More small size, More right side\n");
                    SetMotorLR(motorL, motorR, 4525); // Strong right turn
                }
            } else if (RED_HIGH_WIDTH <= red_size){ // too large
                debug_print("Red maker is too large\n");
                SetMotorLR(motorL, motorR, 1515);
            }
        }
    }
    
    Stopped_Zybot(motorL, motorR);

    // Reffered to http://d.hatena.ne.jp/mFumi/20101002/1286003738
    tcsetattr(0,TCSANOW,&save_settings);
    return(0);
}

  1. 2016年12月01日 04:16 |
  2. Zybot
  3. | トラックバック:0
  4. | コメント:0

コメント

コメントの投稿


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

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