Zybot による障害物回避

Zybot による障害物回避が曲がりなりにもできた。

今のところ、超音波距離センサで距離を測って一定の距離以下だったら右旋回か左旋回をするようにしている。ただし、元の方向には戻れない。戻るすべがない状態だ。これについては電子コンパスを付けようと思っている。光誘導でもよいが。。。

右旋回をするか？左旋回をするか？は今のところターゲットの障害物がどの位置にあるかを画像で検知している。つまり青いゴミ箱が画像の右側にあれば左旋回でよける。左側にあれば右旋回でよける。青いゴミ箱の検知にはRGB2HSV IP を使って画像を HSV に変換して、H をある一定幅で検知している。それがどっちにあるかでよける方向を決めている。今のところ、青いゴミ箱しか障害物回避できないが、H が同じ幅である一定数ある場合は、それを物体として認知して、その物体の無い方向によけることもできると思う。

実際の動画をお見せしようと思う。まずは、Zybot による障害物回避の右旋回だ。動画の最初にZybotがごみ箱に向かって、ごみ箱の真正面よりも少し右側にずれていることに注目してほしい。この様な時には右旋回する。


次に、Zybot による障害物回避の左旋回。動画の最初にZybotがごみ箱に向かって、ごみ箱の真正面よりも少し左側にずれていることに注目してほしい。この様な時には左旋回する。


最後に障害物回避のアプリケーションソフトの obstacle_avoid.cpp を貼っておく。

// obstacle_avoid.cpp : Obstacle avoidance
// 2017/02/13 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 <termios.h>

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

#define DIR_LEFT_NORMAL        1
#define DIR_LEFT_REVERSE    0
#define DIR_RIGHT_NORMAL    0q
#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 CHECK_LINE_NUMBER    290

#define OBSTACLE_H_HIGH_LIMIT    200
#define OBSTACLE_H_LOW_LIMIT    160

#define SEARCH_OBSTACLE_THRESHOLD    15

#define OBSTACLE_AVOID_DISTANCE            60.0    // cm
#define OBSTACLE_AVOID_NEAR_DISTANCE    30.0    // cm

//#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 Obstacle
// return 0 -- Success, return 1 -- I can not find a marker.
// 
int SearchObstacle(volatile unsigned int *addr, int &obstacle_size, int &obstacle_start){
    enum STATE {IDLE, FIND_OBSTACLE};
    enum STATE cs = IDLE;
    int h = 0;

    obstacle_size = 0;
    for (int i=0; i<SVGA_HORIZONTAL_PIXELS; i++){
        h = (addr[i]/65536) & 0x1ff; // H select
#ifdef DEBUG
        if (i==SVGA_HORIZONTAL_PIXELS/2)
            printf("H = %d\n", h);
#endif

        switch (cs){
            case IDLE :
                if (obstacle_size<SEARCH_OBSTACLE_THRESHOLD){
                    if (h<OBSTACLE_H_HIGH_LIMIT && h>OBSTACLE_H_LOW_LIMIT){
                        obstacle_size++;
                    } else {
                        obstacle_size = 0;
                    }
                } else {
                    cs = FIND_OBSTACLE;
                }
                break;
            case FIND_OBSTACLE :
                if (h<OBSTACLE_H_HIGH_LIMIT && h>OBSTACLE_H_LOW_LIMIT){ // obstacle
                    if (obstacle_size == SEARCH_OBSTACLE_THRESHOLD){
                        if (i>=SEARCH_OBSTACLE_THRESHOLD)
                            obstacle_start = i-SEARCH_OBSTACLE_THRESHOLD;
                        else // error
                            obstacle_start = 0;
                    }
                    obstacle_size++;
                } else {
                    return(0); // end
                }
                break;
        }
    }
        
    return(1); // error
}

// ultrasonic sensor detect
float us_sensor_det(volatile unsigned int *ultrasonic_sensor_inf_0){
    ultrasonic_sensor_inf_0[0] = 0x1; // ap_start = 1
    
    while((ultrasonic_sensor_inf_0[0] & 0x2) == 0) // wait ap_done
        usleep(500); // 5 us
        
    if(int ret_val = (int)ultrasonic_sensor_inf_0[4]) // 0x10 : Data signal of ap_return
        printf("Error: %d\n", ret_val);
    
    float measure = (float)ultrasonic_sensor_inf_0[6] * 0.000342 / 2.0; // 0x18 : Data signal of count_val
#ifdef DEBUG
    printf("Distance is %.2f cm\n", measure);
#endif
    return(measure);
}

// 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 obstacle_size, obstacle_start, obstacle_center;
    struct termios save_settings;
    struct termios settings;
    volatile unsigned int *ultrasonic_sensor_inf_0;
    int fd16;
    int distance;

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

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


    // main loop
    printf("Obstacle avoidance.\n");
    printf("if pushed 'q' key then exit.\n");
    SetMotorLR(motorL, motorR, 25, 25); // Slow Straight

    while(1){
        if ((c = getchar()) != EOF){
            if (c == 'q'){
                SetMotorLR(motorL, motorR, 0, 0); // Stop
                break;
            }
        }

        if ((distance=us_sensor_det(ultrasonic_sensor_inf_0)) <= OBSTACLE_AVOID_DISTANCE){
            int result = SearchObstacle(&frame_buffer[SVGA_HORIZONTAL_PIXELS*CHECK_LINE_NUMBER], obstacle_size, obstacle_start);
            obstacle_center = obstacle_start + obstacle_size/2;
#ifdef DEBUG
            printf("obstacle_size = %d, obstacle_start = %d, obstacle_center = %d\n", obstacle_size, obstacle_start, obstacle_center);
#endif
            if (distance <= OBSTACLE_AVOID_NEAR_DISTANCE){
                if (obstacle_center <= SVGA_HORIZONTAL_PIXELS/2){
                    debug_print("Strong right turn\n");
                    SetMotorLR(motorL, motorR, 45, 25); // Strong right turn                
                } else {
                    debug_print("Strong left turn\n");
                    SetMotorLR(motorL, motorR, 20, 45); // Strong left turn                
                }
            } else {
                if (obstacle_center <= SVGA_HORIZONTAL_PIXELS/2){
                    debug_print("Right turn\n");
                    SetMotorLR(motorL, motorR, 40, 20); // Right turn                
                } else {
                    debug_print("Left turn\n");
                    SetMotorLR(motorL, motorR, 15, 40); // left turn                
                }
            }
        } else {
            SetMotorLR(motorL, motorR, 25, 25); // Slow Straight
        }
    }
    // Reffered to http://d.hatena.ne.jp/mFumi/20101002/1286003738
    tcsetattr(0,TCSANOW,&save_settings);
    return(0);
}