FC2カウンター FPGAの部屋 Zybot

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

FPGAの部屋

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

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, 2525); // Slow Straight

    while(1){
        if ((c = getchar()) != EOF){
            if (c == 'q'){
                SetMotorLR(motorL, motorR, 00); // 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, 4525); // Strong right turn                
                } else {
                    debug_print("Strong left turn\n");
                    SetMotorLR(motorL, motorR, 2045); // Strong left turn                
                }
            } else {
                if (obstacle_center <= SVGA_HORIZONTAL_PIXELS/2){
                    debug_print("Right turn\n");
                    SetMotorLR(motorL, motorR, 4020); // Right turn                
                } else {
                    debug_print("Left turn\n");
                    SetMotorLR(motorL, motorR, 1540); // left turn                
                }
            }
        } else {
            SetMotorLR(motorL, motorR, 2525); // Slow Straight
        }
    }
    // Reffered to http://d.hatena.ne.jp/mFumi/20101002/1286003738
    tcsetattr(0,TCSANOW,&save_settings);
    return(0);
}

  1. 2017年02月22日 04:23 |
  2. Zybot
  3. | トラックバック:0
  4. | コメント:0

Zybotの障害物検知と回避1(超音波距離センサの性能)

Zybotの障害物検知と回避で使用するために、超音波距離センサの指向性を計ってみた。

まずは、現在のZybot の形状を貼っておく。
Zybot_1_170206.jpg

超音波距離センサはZybot のシャーシの下側に付いているので、床に置くと短い距離しか測れないので、平ワッシャをかませて、上向きにした。
Zybot_2_170206.jpg

指向性の測定方法としては、丸ごみ箱をZybot からある距離の正面に置いて距離を測定する。次に左側に位置をずらしていくと、あるところで丸ごみ箱の距離を測定できなくなる。そこまでのずらした距離を測定する。右側も同様に行う。

丸ごみ箱の寸法 下直径 22 cm、上直径 27.5 cm、高さ 34.5 cm
Zybotの寸法 幅 20 cm、長さ 34 cm、ベースの高さ 5.4 cm

Zybot と 丸ごみ箱の距離 60 cm
なお、これが、距離 60 cm の正面での超音波距離センサの測定の様子だ。
Zybot_3_170206.jpg

Zybot_4_170206.jpg

次に、距離 60 cm の左 30 cm での超音波距離センサの測定の様子だ。
Zybot_5_170206.jpg

正面でのZybot と丸ごみ箱の距離 62 cm
左 15 cm での距離 64 cm
左 30 cm での距離 70 cm
左 45 cm での距離 222 cm (ここで丸ごみ箱を検出できなくなる)

右 15 cm での距離 64 cm
右 30 cm での距離 70 cm
右 45 cm での距離 77 cm
右 50 cm での距離 222 cm (ここで丸ごみ箱を検出できなくなる)

右のほうが遠くまで検出できている。2つあるユニットの右側が受信なのか?

Zybot と 丸ごみ箱の距離 90 cm
正面でのZybot と丸ごみ箱の距離 92 cm
左 15 cm での距離 94 cm
左 30 cm での距離 98 cm
左 45 cm での距離 104 cm
左 60 cm での距離 204 cm (114 cm が混じっている)
左 64 cm での距離 204 cm (ここで丸ごみ箱を検出できなくなる)

右 15 cm での距離 94 cm
右 30 cm での距離 97 cm
右 45 cm での距離 103 cm
右 60 cm での距離 204 cm (114 cm が混じっている)
右 66 cm での距離 205 cm (ここで丸ごみ箱を検出できなくなる)

Zybot と 丸ごみ箱の距離 30 cm

ここでも測定している写真を撮った。
Zybot_6_170206.jpg

Zybot_7_170206.jpg

Zybot と 丸ごみ箱の距離 30 cm
正面でのZybot と丸ごみ箱の距離 31.7 cm
左 15 cm での距離 37 cm (222 cm も混じっている)
左 22 cm での距離 222 cm (ここで丸ごみ箱を検出できなくなる)

右 15 cm での距離 35 cm
右 30 cm での距離 203 cm (ここで丸ごみ箱を検出できなくなる)

ここで言えることは、30 cm まで接近したのでは、Zybot の距離センサでは、30 cm の距離に近づくと回避したとしてむ、回避しきれたかどうか超音波距離センサだけでは、分からないということだ。

よけきれかどうか超音波距離センサで検出するためには、回避動作を始める距離は少なくとも 60 cm くらいにしておく必要があるかもしれない。どちらの方向に回避するか?回避しきれたかどうか?は画像処理で行うようにしよう。
  1. 2017年02月06日 12:40 |
  2. Zybot
  3. | トラックバック:0
  4. | コメント:0

Zybot をステレオカメラにする3(Zybot 改造中)

Zybot をステレオカメラに改造中です。

ZYBO を2枚搭載して、それぞれにカメラを付けてZYBO 同士のHDMI コネクタをケーブルで接続しました。片方ZYBO のカメラの画像をもう一方のZYBO に転送して、以前、やっていたステレオカメラにします。カメラ用のマウントは3Dプリンタで作製した”Zybot をステレオカメラにする2(カメラ・マウントの作製)”のマウントを使用しています。
Zybot_1_170118.jpg

Zybot_2_170118.jpg

Zybot_3_170118.jpg

バッテリーは今まではリチウムイオン電池のジャンプスターター1個でしたが、+5Vのモバイルバッテリーを追加して、ZYBOの電源などはそこから取ろうと思っています。
問題は無線LANで、今までWLI-UTX-AG300を使っていて、これにスイッチングハブを付けてZYBO 2台のLANコネクタに接続してみたのですが、通信できませんでした。WLI-UTX-AG300を2つというのも考えたのですが、大きすぎます。そこで、ZYBOのUSB コネクタに付けられないかな?と思い、WN-G150U を買ってみました。果たしてZYBO が無線LAN親機につながるでしょうか?
超ド級の迫力のZybot になってしまいました。。。
果たして走るのでしょうか?
  1. 2017年01月18日 16:10 |
  2. Zybot
  3. | トラックバック:0
  4. | コメント:0

Zybot をステレオカメラにする2(カメラ・マウントの作製)

Zybot をステレオカメラにする1(カメラ・インターフェース基板の実装とテスト)”の続き。

ずいぶんと間が空いてしまったが、前回新しくI2Cリピーター付きのカメラ・インターフェース基板を作って、延長ケーブルを付けてカメラが正常に写った。今回は、カメラを付けたカメラ・インターフェース基板を搭載できるカメラ・マウントのモデルを3D CADで作成し、3Dプリンタで作製した。

最初に、FreeCAD でカメラ・インターフェース基板をZybot のベース・プレートに付けるためのカメラ・マウントのモデルを作成した。
stereo_cam_mounter_1_170105.png

次に、3Dプリンタで印刷した。充填率を 25 % にしたら、表面に細かい穴が空いてしまった。
stereo_cam_mounter_2_170106.jpg

次に充填率を 50 % にして印刷した。こちらは、細かい穴が目立たない。
stereo_cam_mounter_3_170106.jpg

2つ並べたところ。左が充填率 25 % で、右が充填率を 50 % だ。
stereo_cam_mounter_4_170106.jpg
stereo_cam_mounter_5_170106.jpg

やはり、充填率が 25 % だとボツボツなので、50 % のものをもう1つ作って使うことにした。

カメラ・インターフェース基板をカメラ・マウントに取り付けてみた。
stereo_cam_mounter_6_170106.jpg

こんな感じ。(Zybot のベース・プレートに付ける予定ですが、まだ付けていないです)
  1. 2017年01月06日 21:52 |
  2. Zybot
  3. | トラックバック:0
  4. | コメント:0

Zybot に超音波距離センサを搭載する10(Zybot に取り付けた)

Zybot に超音波距離センサを搭載する9(IP のUIO番号を確認)”の続き。

前回までで、超音波距離センサ・インターフェースIP の動作が確認できて、Linux のUIO ドライバを使えるように設定した。今回は、Zybot に超音波距離センサを搭載して、Linux 上で動作を確認した。

まずは、超音波距離センサをユニバーサル基板に搭載してピンヘッダを付けた。ピンヘッダからブレッドボード・ジャンパーコード(オス-メス)でZYBO のPmod JB まで接続した。ジャンパーコードの色は変えてある。+3.3V は赤、GND は黒、SIG は青にしてある。
その取り付けや接続の様子を写真で示す。
Ultrasonic_senser_inf_73_161220.jpg

Ultrasonic_senser_inf_74_161220.jpg

これで前回作ったBOOT.bin と devicetree.dtb をMicro SD カードに入れて、ZYBO に挿入して電源をON して、Ubuntu 14.04 を起動した。
Zybot ディレクトリの下にultrasonic_sensor ディレクトリを作って、cam_disp_plat などを入れた。隊列走行の時とは、超音波距離センサのエントリがデバイスツリーに増えただけなので、そのままカメラを表示することができた。
Ultrasonic_senser_inf_71_161219.png

その下に、us_sensor_test ディレクトリを作成した。us_sensor_test.c を作成して、gcc -o us_sensor_test us_sensor_test.c でコンパイルした。us_sensor_test が生成された。
Ultrasonic_senser_inf_72_161219.png

./us_sensor_test でアプリケーションを起動すると、距離を測定することができた。
Ultrasonic_senser_inf_70_161219.png

us_sensor_test.c を示す。

// us_sensor_test.c
// 2016/12/19 by marsee
// 

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>

#include "xultrasonic_sensor_inf.h"

void main(){
    int fd16;
    volatile unsigned int *ultrasonic_sensor_inf_0;
    int ret_val;
    float measure;
    
    // 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);
    }
    
    while(1){
        usleep(200000); // 200 ms
        
        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(ret_val = (int)ultrasonic_sensor_inf_0[4]) // 0x10 : Data signal of ap_return
            printf("Error: %d\n", ret_val);
        
        measure = (float)ultrasonic_sensor_inf_0[6] * 0.000342 / 2.0// 0x18 : Data signal of count_val
        printf("Distance is %.2f cm\n", measure);
    }
    munmap((void *)ultrasonic_sensor_inf_0, 0x10000);
    close(fd16);
}



これで、超音波距離センサ・インターフェースIP をLinuxからも使うことができた。
  1. 2016年12月20日 04:29 |
  2. Zybot
  3. | トラックバック:0
  4. | コメント:0

Zybot に超音波距離センサを搭載する9(IP のUIO番号を確認)

Zybot に超音波距離センサを搭載する8(ZYBO_0_162_7 に 2 つのIP を追加2)”の続き。

前回は、超音波距離センサ・インターフェースIP が完成して、SDKからベアメタル・アプリケーションで動作を確認した。今回は、BOOT.bin と devicetree.dtb を作成して、SDカードに書き込み、ZYBO で UIO としての動作を確認する。

まずはSDK でFSBL を作成した。

FSBL のアプリケーション・プロジェクトで、右クリックし、右クリックメニューから Create Boot Image を選択して、BOOT.bin を作成した。
Ultrasonic_senser_inf_64_161217.png

BOOT.bin が作成された。
Ultrasonic_senser_inf_65_161217.png

VirtualBox 上のUbuntu 14.04 で、drivers_ZYBO_0_162_6 を drivers_ZYBO_0_162_7 にコピーして、zynq-zybo.dts に ultrasonic_sensor_inf_0 のエントリを追加した。
Ultrasonic_senser_inf_66_161217.png

dtc -I dts -O dtb -o devicetree.dtb zynq-zybo.dts コマンドで devicetree.dtb が生成された。
Ultrasonic_senser_inf_67_161217.png

BOOT.bin と devicetree.dtb をZYBO_BOOT のMicro SD カードに書き込んで、ZYBO にセットし、電源ON。

/sys/devices/amba.0 に行くと、ultrasonic_sensor_inf_0 のエントリがあった。
Ultrasonic_senser_inf_68_161217.png

ultrasonic_sensor_inf_0 は uio16 だった。
Ultrasonic_senser_inf_69_161217.png
  1. 2016年12月17日 05:42 |
  2. Zybot
  3. | トラックバック:0
  4. | コメント:0

Zybot に超音波距離センサを搭載する8(ZYBO_0_162_7 に 2 つのIP を追加2)

Zybot に超音波距離センサを搭載する7(ZYBO_0_162_7 に 2 つのIP を追加)”の続き。

前回はVivado HLS 2016.3 の Ultrasorinc_sensor_inf IP とussensor_iobuf IP をZYBO_0_162_7 フォルダのプロジェクトのブロックデザインに追加して、論理合成、インプリメント、ビットストリームの生成を行った。
今回は、SDKでアプリケーション・ソフトウェアを作成して、ZYBO でテストを行った。

SDKで、ultrasonic_sensor_inf アプリケーション・プロジェクトを作成して、ultrasonic_sensor.c を作成した。
Ultrasonic_senser_inf_59_161216.png

ultrasonic_sensor.c を示す。

// ultrasonic_sensor.c
// 2016/12/14 by marsee
//

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include "xil_io.h"
#include "xparameters.h"

#include "xultrasonic_sensor_inf.h"

int main(){
    XUltrasonic_sensor_inf uss_inf;
    int ret_val;
    float measure;
    
    XUltrasonic_sensor_inf_Initialize(&uss_inf, 0);
    
    while(1){
        usleep(200000); // 200 ms

        XUltrasonic_sensor_inf_Start(&uss_inf);

        while(!XUltrasonic_sensor_inf_IsDone(&uss_inf))
            usleep(500); // 5us

        if(ret_val = (int)XUltrasonic_sensor_inf_Get_return(&uss_inf))
            printf("Error: %d\n", ret_val);

        measure = (float)XUltrasonic_sensor_inf_Get_count_val(&uss_inf) * 0.000342 / 2.0;
        printf("Distance is %.2f cm\n", measure);
    }
}


ultrasonic_sensor.elf の実行結果を示す。
Ultrasonic_senser_inf_58_161215.png

うまく距離が測定できた。

しかし。。。距離は測定できたのだが、たまに動かなくなってしまう。5分くらいで動作しなくなってしまう。動作しなくなったら、再コンフィギュレーションしないと再び動き出さない。
2時間くらい悩んでいたのだが、ふと気が付いた。sensor_in に入って来る超音波距離センサの距離測定パルスは非同期なのに、シンクロナイザが入っていなかった。Vivado HLS で生成されるIP にシンクロナイザを望むべくもないので、自分で作った、Synchronizer を sensor_in に追加することにした。

Synchronizer を追加した階層モジュール usoinc_sensor_inf を示す。
Ultrasonic_senser_inf_60_161216.png

Synchronizer はビット幅 1 、ステージ数 2 に設定されている。
Ultrasonic_senser_inf_61_161216.png

これでもう一度、論理合成、インプリメント、ビットストリームの生成を行った。
レポートを示す。
Ultrasonic_senser_inf_62_161216.png

成功したので、ハードウェアをエクスポートして、SDK を立ち上げた。

ultrasonic_sensor.elf を実行したところ、30分経っても動作は安定していた。やはり、Synchronizer が無かったためメタステーブル状態が発生し、ステートマシンが不正ステートに行ってしまって、回復できなかったのが原因のようだ。
これで、超音波距離センサをZybot に搭載することができる。。。やった~~~。

なお、実験方法はZYBO のPmod の +3.3V を パララックス社の超音波距離センサの+5V に入力している。つまり、データシートの規格から逸脱して使っているが、今のところは問題なく使用できている。SIG はPmod のJB の 7 番ピン Y18 に接続している。
接続してる写真を示す。あとはGNDを接続している。配線だけのシンプルな回路だ。
Ultrasonic_senser_inf_63_161216.jpg
  1. 2016年12月16日 04:46 |
  2. Zybot
  3. | トラックバック:0
  4. | コメント:0
»