Zybot でガボール・フィルタを使用して白線間走行する15（ZYBOが壊れた？）

”Zybot でガボール・フィルタを使用して白線間走行する14（コースを新設）”の続き。

Zybot でガボール・フィルタを使用して白線間を走行させたいと思っているということで、前回は、コースを新設してやってみた。ロボットカーを走らせてみたところ、コース沿いに左に曲がったが、曲がりすぎてしまう。今回は、白線の幅を 19 cm から 21 cm に変更したが、ZYBO の USB コネクタが怪しくなり、QSPI フラッシュと SD カードからのブートに問題が出てしまった。

USB ケーブルを Zybot に接続して FPGA を JTAG からコンフィグレーションしてテストをしてきたが、いよいよコネクタの接触が怪しくなった。コンフィグレーションできなくなったし、USB コネクタを持ち上げないと電源が入らなくなってしまった。

更に QSPI フラッシュと SD カードからのブートの時のガボール・フィルタの画像がおかしい。どちらもノイズが乗りまくっている画像になってしまっている。


何ででしょう？

ZYBO ボードの調子が悪いので、ペンディングとして、KR260 をやってみようと思う。

Zybot でガボール・フィルタを使用して白線間走行する14（コースを新設）

”Zybot でガボール・フィルタを使用して白線間走行する13（白線判断のポイントを確認する）”の続き。

Zybot でガボール・フィルタを使用して白線間を走行させたいと思っているということで、前回は、白線の位置を判断するポイントがロボットカーからどの程度離れているのか？を確認する。確認の結果、ロボットカーの先端から約 27 cm の位置で判定しているようだった。今回は、コースを新設してやってみた。ロボットカーを走らせてみたところ、コース沿いに左に曲がったが、曲がりすぎてしまう。

前回は白線間のコース幅が 21 cm だったと思うが、今回は、19 cm 幅のコースを白い紙テープで作成してみた。


ロボットカーを直進する位置に置いた。

ガボール・フィルタとラプラシアン・ファイルをスルーしたカメラ画像のディスプレイ表示を示す。


シリアル・インターフェース経由で取得したテキスト・ファイルを画像ファイルに変換した画像を示す。


左白線用パラメータのガボール・フィルタ画像を示す。


シリアル・インターフェース経由で取得したテキスト・ファイルを画像ファイルに変換した画像を示す。


右白線用パラメータのガボール・フィルタ画像を示す。
写真がぶれてしまっている。


シリアル・インターフェース経由で取得したテキスト・ファイルを画像ファイルに変換した画像を示す。


これで走らせてみたところ、左に曲がりすぎてしまう。
スピードが速いのと白線幅が狭いので、左に曲がった際に白線を認識できなくなってしまうのかもしれない？
白線幅を 21 cm にして、以前のモーターの PWM 値で試してみよう。

Zybot でガボール・フィルタを使用して白線間走行する13（白線判断のポイントを確認する）

”Zybot でガボール・フィルタを使用して白線間走行する12（ラプラシアン・フィルタを外してガボール・フィルタだけにする）”の続き。

Zybot でガボール・フィルタを使用して白線間を走行させたいと思っているということで、前回は、ラプラシアン・フィルタを外してガボール・フィルタだけにしてみたが、白線が認識されているようだ。やはり、キャッシュをインバリデートしていなかったのが痛かった。今回は、白線の位置を判断するポイントがロボットカーからどの程度離れているのか？を確認する。確認の結果、ロボットカーの先端から約 27 cm の位置で判定しているようだった。

今回は、白線判断のポイントを確認するために、ガボール・フィルタもラプラシアン・フィルタもキャンセルして、通常のカメラ画像で確認する。
wl_tracing_gabor_bm.c の 221 行目の

gabor_filter_lh_3_0[10] = (volatile uint32_t)1; // function_r, Gabor_Filter IP ON

をコメントアウトして、下の行に

gabor_filter_lh_3_0[10] = (volatile uint32_t)0; // function_r, Gabor_Filter IP OFF

を追加した。


最初にロボットカーをコース上に適当に置いてカメラ画像を確認する。


ディスプレイに映った画像を示す。


カメラ画像を取得する。
シリアル・ターミナルでは、一部のデータしか見ることができないので、cu コマンドでファイルに保存する。
最初に /ttyUSB1 の権限を変更した。
sudo chmod o+wr /dev/ttyUSB1

カメラ画像データを
cu -s 115200 -l /dev/ttyUSB1 > line1_230111.txt
で取得した。
終了は ~. を入力した。

取得したテキスト・ファイルを bmp ファイルに変換する。（”テキスト・ファイルを画像ファイル（bmp ファイル）に変換するソフトウェアを作成する”を参照）
./txt2bmp line1_230111.txt line1_230111.bmp


line1_230111.bmp を示す。


ロボットカーから白線判断のポイントが結構離れているようなので、大体の距離を測定する。
ロボットカーのカメラから白線がカーブから直線になるポイントを映すようにロボットカーの位置を微調整した。


ロボットカーからのカメラ画像を示す。


距離を測定すると、ロボットカーの先端から白線がカーブから直線になるポイントの距離は大体 27 cm 程度だった。

カメラ画像を取得する。
シリアル・ターミナルでは、一部のデータしか見ることができないので、cu コマンドでファイルに保存する。
最初に /ttyUSB1 の権限を変更した。
sudo chmod o+wr /dev/ttyUSB1

カメラ画像データを
cu -s 115200 -l /dev/ttyUSB1 > line2_230111.txt
で取得した。
終了は ~. を入力した。

取得したテキスト・ファイルを bmp ファイルに変換する。（”テキスト・ファイルを画像ファイル（bmp ファイル）に変換するソフトウェアを作成する”を参照）
./txt2bmp line2_230111.txt line2_230111.bmp


line2_230111.bmp を示す。

Zybot でガボール・フィルタを使用して白線間走行する12（ラプラシアン・フィルタを外してガボール・フィルタだけにする）

”Zybot でガボール・フィルタを使用して白線間走行する11（アプリケーション・ソフトを更新してテスト）”の続き。

Zybot でガボール・フィルタを使用して白線間を走行させたいと思っているということで、前回は、ハードウェアをエクスポートして、Vitis を立ち上げアプリケーション・ソフトウェアをハードウェアの変更に合わせて更新し、Zybot でテストしたところうまく行っているようだった。今回は、ラプラシアン・フィルタを外してガボール・フィルタだけにしてみたが、白線が認識されているようだ。やはり、キャッシュをインバリデートしていなかったのが痛かった。

前回までは、ガボール・フィルタをかけてからラプラシアン・フィルタをかけていたが、今回は、ラプラシアン・フィルタをスルーにしてガボール・フィルタだけをかけてみよう。
wl_tracking_gabor_bm.c の 226行目の

lap_filter_axis_0[10] = (volatile uint32_t)1; // function_r, Lap_Filter IP ON

をコメントアウトして、下の行に

lap_filter_axis_0[10] = (volatile uint32_t)0; // function_r, Lap_Filter IP OFF

を追加した。


これでビルドして、ZYBO に書き込んだ。

ガボール・フィルタの左白線用パラメータの画像と右白線用パラメータの画像を取得する。
シリアル・ターミナルでは、一部のデータしか見ることができないので、cu コマンドでファイルに保存する。
最初に /ttyUSB1 の権限を変更した。
sudo chmod o+wr /dev/ttyUSB1

左白線パラメータでのガボール・フィルタのデータを
cu -s 115200 -l /dev/ttyUSB1 > gabor_left1_230110.txt
で取得した。
終了は ~. を入力した。

右白線パラメータでのガボール・フィルタのデータを
cu -s 115200 -l /dev/ttyUSB1 > gabor_right1_230110.txt
で取得した。
終了は ~. を入力した。

取得したテキスト・ファイルを bmp ファイルに変換する。（”テキスト・ファイルを画像ファイル（bmp ファイル）に変換するソフトウェアを作成する”を参照）
txt2bmp 実行ファイルを使用して、gabor_left2_230107.txt と gabor_right2_230107.txt を画像ファイル（bmp ファイル）に変換する。
./txt2bmp gabor_left1_230110.txt gabor_left1_230110.bmp
./txt2bmp gabor_right1_230110.txt gabor_right1_230110.bmp
bmp ファイルが生成された。




gabor_left1_230110.bmp と gabor_right1_230110.bmp を示す。
ラプラシアン・ファイルを更にかけるよりも、ガボール・フィルタだけのほうが綺麗な画像になっている。

テキスト・ファイルを画像ファイル（bmp ファイル）に変換するソフトウェアを作成する

”Zybot でガボール・フィルタを使用して白線間走行する11（アプリケーション・ソフトを更新してテスト）”で作成した gabor_left2_230107.txt と gabor_right2_230107.txt を画像ファイル（bmp ファイル）に変換するソフトウェアを作成し、変換した。

テキスト・ファイルを画像ファイル（bmp ファイル）に変換するソフトウェア txt2bmp.c を示す。

// txt2bmp.c
// 2023/01/08 by marsee

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <fcntl.h>
#include <stdint.h>
#include <string.h>

#include "bmp_header.h"

#define HORIZONTAL_PIXEL    800
#define VERTICAL_LINES      600

int main(int argc, char * argv[])
{
    BITMAPFILEHEADER bmpfh; // BMPファイルのファイルヘッダ
    BITMAPINFOHEADER bmpih; // BMPファイルのINFOヘッダ
    FILE *fbmpr, *fbmpw;
    uint8_t linet[200];
    uint8_t num_buf[10], c;
    uint32_t *bmp_data;
    uint8_t blue, green, red;

    // 引数の処理
    if (argc != 3) {
        fprintf(stderr, "txt2bmp <txt file name> <BMP file name>\n");
        exit(1);
    }

    // ピクセルを入れるメモリをアロケートする
    if ((bmp_data =(uint32_t *)malloc(sizeof(uint32_t) * (HORIZONTAL_PIXEL*VERTICAL_LINES))) == NULL){
        fprintf(stderr, "Can't allocate rd_bmp memory\n");
        exit(1);
    }

    // text ファイルをオープン
    if ((fbmpr = fopen(argv[1], "rt")) == NULL){
        fprintf(stderr, "Can't open %s by text read mode\n", argv[1]);
        exit(1);
    }

    // 書き込み bmp ファイルをオープン
    if ((fbmpw=fopen(argv[2], "wb")) == NULL){
        fprintf(stderr, "Can't open %s binary write mode\n", argv[2]);
        exit(1);
    }

    fscanf(fbmpr, "%s\n", linet);
    uint32_t line_num = (uint32_t)strlen(linet)/2;
    uint32_t limit_line = (HORIZONTAL_PIXEL*VERTICAL_LINES+(line_num-1))/line_num;
    fseek(fbmpr, 0, SEEK_SET); // ポインタ位置をファイルのはじめに戻す
    for (int i=0; i<limit_line; i++){
        fscanf(fbmpr, "%s\n", linet);
        uint32_t cline_char = (uint32_t)strlen(linet);
        for (int j=0; j<cline_char; j+=2){
            strncpy(num_buf, &linet[j], 2);
            num_buf[2] = '\0';
            sscanf(num_buf, "%x", (const char *)&c);
            bmp_data[i*line_num+j/2] = (c<<16)+(c<<8)+c;
        }
    }

    // BMPファイルのファイルヘッダに値を代入
    bmpfh.bfType = 0x4d42;
    bmpfh.bfSize = HORIZONTAL_PIXEL*VERTICAL_LINES*3+54;
    bmpfh.bfReserved1 = 0;
    bmpfh.bfReserved2 = 0;
    bmpfh.bfOffBits = 0x36;
    // BMPファイルのINFOヘッダに値を代入
    bmpih.biSize = 0x28;
    bmpih.biWidth = HORIZONTAL_PIXEL;
    bmpih.biHeight = VERTICAL_LINES;
    bmpih.biPlanes = 0x1;
    bmpih.biBitCount = 24;
    bmpih.biCompression = 0;
    bmpih.biSizeImage = 0;
    bmpih.biXPixPerMeter = 3779;
    bmpih.biYPixPerMeter = 3779;
    bmpih.biClrUsed = 0;
    bmpih.biClrImporant = 0;

    // BMPファイルヘッダの書き込み
    fwrite(&bmpfh.bfType, sizeof(uint16_t), 1, fbmpw);
    fwrite(&bmpfh.bfSize, sizeof(uint32_t), 1, fbmpw);
    fwrite(&bmpfh.bfReserved1, sizeof(uint16_t), 1, fbmpw);
    fwrite(&bmpfh.bfReserved2, sizeof(uint16_t), 1, fbmpw);
    fwrite(&bmpfh.bfOffBits, sizeof(uint32_t), 1, fbmpw);
    // BMPファイルのINFOヘッダの書き込み
    fwrite(&bmpih, sizeof(BITMAPINFOHEADER), 1, fbmpw);
    // bmp_dataの書き込み
    for (int y=0; y<bmpih.biHeight; y++) {
        for (int x=0; x<bmpih.biWidth; x++) {
            blue = bmp_data[((bmpih.biHeight-1)-y)*bmpih.biWidth+x] & 0xff;
            green = (bmp_data[((bmpih.biHeight-1)-y)*bmpih.biWidth+x] >> 8) & 0xff;
            red = (bmp_data[((bmpih.biHeight-1)-y)*bmpih.biWidth+x]>>16) & 0xff;

            fputc(blue, fbmpw);
            fputc(green, fbmpw);
            fputc(red, fbmpw);
        }
    }
    fclose(fbmpw);
    free(bmp_data);

    return 0;
}

~/ドキュメント/temp ディレクトリに gabor_left2_230107.txt と gabor_right2_230107.txt があるので、そこに txt2bmp.c と bmp_header.h をコピーした。


gcc を使ってコンパイルした。
gcc -o txt2bmp txt2bmp.c
ワーニングは出たが、txt2bmp 実行ファイルが生成された。


txt2bmp 実行ファイルを使用して、gabor_left2_230107.txt と gabor_right2_230107.txt を画像ファイル（bmp ファイル）に変換する。
./txt2bmp gabor_left2_230107.txt gabor_left2_230107.bmp
./txt2bmp gabor_right2_230107.txt gabor_right2_230107.bmp
bmp ファイルが生成された。


生成された gabor_left2_230107.bmp と gabor_right2_230107.bmp を示す。
bmp ファイルの生成に成功した。

Zybot でガボール・フィルタを使用して白線間走行する11（アプリケーション・ソフトを更新してテスト）

”Zybot でガボール・フィルタを使用して白線間走行する10（作成した IP をブロック・デザインに組み込む）”の続き。

Zybot でガボール・フィルタを使用して白線間を走行させたいと思っているということで、前回は、作ってきた IP を ZYBOt ブロック・デザインに組み込んで、論理合成、インプリメンテーション、ビットストリームの生成を行って成功した。今回は、ハードウェアをエクスポートして、Vitis を立ち上げアプリケーション・ソフトウェアをハードウェアの変更に合わせて更新し、Zybot でテストしたところうまく行っているようだった。

Vivado 2022.2 でハードウェアをエクスポートして、ZYBOt_wrapper.xsa を再作成した。
Vivado の Tools メニューから Launch Vitis IDE を選択して、Vitis 2022.2 を起動した。
ワークスペースを ZYBO_222/vitis_work に指定した。
Vitis が立ち上がった。
wl_tracing_gabor_bm_system/wl_tracing_gabor_bm/src/wl_tracing_gabor_bm.c を編集した。


前にやった Zybot 実機を使用したテストでは、最初から全く結果が更新されていなかったが、その原因がやっと分かった。
それは、ベアメタル・アプリケーションとして実行しているため、ARM プロセッサのキャッシュが ON になっていて、メモリに DMA されたデータがキャッシュに反映されていないためだった。今回は、400 ピクセル分を 2 セット読んでいるだけなので、キャッシュにまるごと登録されているようだ。
解決策としてはキャッシュをインバリデートすれば良い。ベアメタル・アプリケーションでは、Xil_DCacheInvalidate() 関数を使用する。


作成した wl_tracing_gabor_bm.c を貼っておく。
とりあえず、ガボール・フィルタもラプラシアン・フィルタも ON してある。

// wl_tracking_gabor_bm.c
// 2022/12/18 : by marsee
// 2022/12/21 : bug fix. by marsee
// 2022/12/23 : Added sw2, BTN0.
// 2023/01/07 : Added Gabor_Filter_lh_3, lap_filter_axis.

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include "sleep.h"

#include "xparameters.h"
#include "xil_io.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            240
#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 FRAME_BUFFER_ADDRESS    0x10000000

#define GABOR_LEFT  0
#define GABOR_RIGHT 1

void cam_i2c_init(volatile uint32_t *mt9d111_axi_iic) {
    mt9d111_axi_iic[64] = 0x2; // reset tx fifo ,address is 0x100, i2c_control_reg
    mt9d111_axi_iic[64] = 0x1; // enable i2c
}

void cam_i2x_write_sync(void) {
    // unsigned c;

    // c = *cam_i2c_rx_fifo;
    // while ((c & 0x84) != 0x80)
        // c = *cam_i2c_rx_fifo; // No Bus Busy and TX_FIFO_Empty = 1
    usleep(1000);
}

void cam_i2c_write(volatile uint32_t *mt9d111_axi_iic, uint32_t device_addr, uint32_t write_addr, uint32_t write_data){
    mt9d111_axi_iic[66] = 0x100 | (device_addr & 0xfe);    // Slave IIC Write Address, address is 0x108, i2c_tx_fifo
    mt9d111_axi_iic[66] = write_addr;
    mt9d111_axi_iic[66] = (write_data >> 8)|0xff;            // first data
    mt9d111_axi_iic[66] = 0x200 | (write_data & 0xff);        // second data
    cam_i2x_write_sync();
}

int search_gabor_edge(uint32_t start_addr, uint32_t number, uint32_t threshold){
    volatile uint32_t *imgaddr;
    int i;

    imgaddr = (volatile uint32_t *)start_addr;
    //printf("start_addr = %x\n", (int)start_addr);

    for (i=0; i<number; i++){
        uint32_t c=imgaddr[i] & 0xff;
        //printf("%d, %d, %d\n",(int)number, i, (int)c);
        //printf("%x",(int)c);
        if (c >= threshold){
            break;
        }
        //usleep(5000);
    }
    //printf("i = %d\n",i);
    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, 0) != XST_SUCCESS){
        fprintf(stderr,"pwm_0 (Left) open error\n");
        exit(-1);
    }
    if (XPwm_Initialize(motorRp, 1) != XST_SUCCESS){
        fprintf(stderr,"pwm_1 (Right) open error\n");
        exit(-1);
    }

    // Initialization of motor monitor
    if (XMotor_monitor_Initialize(mmLp, 0) != XST_SUCCESS){
        fprintf(stderr,"motor_monitor_0 (Left) open error\n");
        exit(-1);
    }
    if (XMotor_monitor_Initialize(mmRp, 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 check_debug(volatile uint32_t *axi_gpio_0){
    uint32_t val = axi_gpio_0[0];
    return(val & 1);
}

int check_motor_on(volatile uint32_t *axi_gpio_0){
    uint32_t val = axi_gpio_0[0];
    val = val>>1;
    return(val & 1);
}

int check_gabor_LR(volatile uint32_t *axi_gpio_0){
    uint32_t val = axi_gpio_0[0];
    val = val>>2;
    return(val & 1);
}

int check_gabor_display(volatile uint32_t *axi_gpio_0){
    uint32_t val = axi_gpio_0[0];
    val = val>>3;
    return(val & 1);
}

int gabor_data_disp(uint32_t fb_addr, uint32_t lr){
    volatile uint32_t *fb_addrp = (volatile uint32_t *)fb_addr;

    for(int i=0; i<SVGA_HORIZONTAL_PIXELS*SVGA_VERTICAL_LINES; i++){
        if((i%40) == 0){
            if (i != 0)
                printf("\n");
        }
        printf("%02x", (int)(fb_addrp[i] & 0xff));
    }
    printf("\n");
    return(0);
}

int main(){
    volatile uint32_t *dmaw4gabor_0;
    XPwm motorL, motorR;
    XMotor_monitor mmL, mmR;
    int left_wl_edge, right_wl_edge;
    // mt9d111_inf_axis_0, axi_iic_0, bitmap_disp_cntrler_axi_master_0
    volatile uint32_t *bmdc0_axi_lites;
    volatile uint32_t *mt9d111_axi_lites;
    volatile uint32_t *mt9d111_i2c_axi_lites;
    volatile uint32_t *axi_gpio_0;
    volatile uint32_t *gabor_filter_lh_3_0;
    volatile uint32_t *lap_filter_axis_0, *find_startp_0;

    // axi_gpio_0 : sw0 - 0 : NORMAL, 1 : DEBUG
    //              sw1 - 0 : MOTOR OFF, 1 : MOTOR ON
    //              sw2 - 0 : Display the Gabor-filtered image for the left white line on the display
    //              sw2 - 1 : Display the Gabor-filtered image for the right white line on the display
    //              BTN0 - 1 : Display the value of the Gabor filter for one screen. sw2 designates right white line data or left white line data.
    axi_gpio_0 = (volatile uint32_t *)XPAR_AXI_GPIO_0_BASEADDR;
    gabor_filter_lh_3_0 = (volatile uint32_t *)XPAR_XGABOR_FILTER_LH_3_0_S_AXI_CONTROL_BASEADDR;
    lap_filter_axis_0 = (volatile uint32_t *)XPAR_XLAP_FILTER_AXIS_0_S_AXI_CONTROL_BASEADDR;
    find_startp_0 = (volatile uint32_t *)XPAR_XFIND_STARTP_0_S_AXI_CONTROL_BASEADDR;

    // Motor Initialize
    motor_initialize(&motorL, &motorR, &mmL, &mmR);

    // DMAW4Gabor Initialize
    dmaw4gabor_0 = (volatile uint32_t *)XPAR_CAMERA_INTERFACE_DMAW4GABOR_0_S_AXI_AXILITES_BASEADDR;

    // DMA4Gabor frame_buffer setting
    dmaw4gabor_0[6] = (volatile uint32_t)FRAME_BUFFER_ADDRESS; // Data signal of frame_buffer0
    dmaw4gabor_0[8] = (volatile uint32_t)FRAME_BUFFER_ADDRESS + (volatile uint32_t)SVGA_ALL_DISP_ADDRESS; // Data signal of frame_buffer1

    // Camera, display controller
    bmdc0_axi_lites = (volatile uint32_t *)XPAR_BITMAP_DISP_CNTRLER_AXI_MASTER_0_BASEADDR;
    mt9d111_axi_lites = (volatile uint32_t *)XPAR_CAMERA_INTERFACE_MT9D111_INF_AXIS_0_BASEADDR;
    mt9d111_i2c_axi_lites = (volatile uint32_t *)XPAR_CAMERA_INTERFACE_AXI_IIC_0_BASEADDR;

    // Gabor, find_startp, lap_filer Settings
    gabor_filter_lh_3_0[6] = (volatile uint32_t)SVGA_VERTICAL_LINES; // row_size
    gabor_filter_lh_3_0[8] = (volatile uint32_t)SVGA_HORIZONTAL_PIXELS; // col_size
    gabor_filter_lh_3_0[10] = (volatile uint32_t)1; // function_r, Gabor_Filter IP ON
    find_startp_0[6] = (volatile uint32_t)SVGA_VERTICAL_LINES; // row_size
    find_startp_0[8] = (volatile uint32_t)SVGA_HORIZONTAL_PIXELS; // col_size
    lap_filter_axis_0[6] = (volatile uint32_t)SVGA_VERTICAL_LINES; // row_size
    lap_filter_axis_0[8] = (volatile uint32_t)SVGA_HORIZONTAL_PIXELS; // col_size
    lap_filter_axis_0[10] = (volatile uint32_t)1; // function_r, Lap_Filter IP ON

    // IP start
    dmaw4gabor_0[0] = (volatile uint32_t)0x81; // start, auto restart
    lap_filter_axis_0[0] = (volatile uint32_t)0x81; // start, auto restart
    gabor_filter_lh_3_0[0] = (volatile uint32_t)0x81; // start, auto restart
    find_startp_0[0] = (volatile uint32_t)0x81; // start, auto restart
    bmdc0_axi_lites[0] = (volatile uint32_t)FRAME_BUFFER_ADDRESS; // Bitmap Display Controller 0 start
    mt9d111_axi_lites[0] = (volatile uint32_t)FRAME_BUFFER_ADDRESS; // Camera Interface start (Address is dummy)

    // CMOS Camera initialize, MT9D111
    cam_i2c_init(mt9d111_i2c_axi_lites);

    cam_i2c_write(mt9d111_i2c_axi_lites, 0xba, 0xf0, 0x1);      // Changed regster map to IFP page 1
    cam_i2c_write(mt9d111_i2c_axi_lites, 0xba, 0x97, 0x20);        // RGB Mode, RGB565

    mt9d111_axi_lites[1] = 0; // One_shot_mode is disabled

    // main loop
    if(check_debug(axi_gpio_0))
        printf("White line Tracking start. \n");

    while(1){
        Xil_DCacheInvalidate();
        // Gabor filter for left white line
        left_wl_edge = SVGA_HORIZONTAL_PIXELS/2 - search_gabor_edge(
                FRAME_BUFFER_ADDRESS+GABOR_DETECT_LINE_ADDR, SVGA_HORIZONTAL_PIXELS/2, GABOR_THRESHOLD);

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

        if(check_debug(axi_gpio_0))
            printf("left_wl_edge = %d, right_wl_edge = %d\n", left_wl_edge, right_wl_edge);

        if (left_wl_edge == LEFT_GABOR_EDGE_OVERFLOW){
            if(check_motor_on(axi_gpio_0)){
                XPwm_Set_sw_late_V(&motorL, 15);
                XPwm_Set_sw_late_V(&motorR, 45);
            }
            if(check_debug(axi_gpio_0))
                printf("Left gabor edge is overflow\n");

        } else if (right_wl_edge == RIGHT_GABOR_EDGE_OVERFLOW){
            if(check_motor_on(axi_gpio_0)){
                XPwm_Set_sw_late_V(&motorL, 45);
                XPwm_Set_sw_late_V(&motorR, 15);
            }
            if(check_debug(axi_gpio_0))
                printf("Right gabar edge is overflow\n");

        } else if ((right_wl_edge - left_wl_edge) > DIST_THRESHOLD){
            if(check_motor_on(axi_gpio_0)){
                XPwm_Set_sw_late_V(&motorL, 35);
                XPwm_Set_sw_late_V(&motorR, 25);
            }
            if(check_debug(axi_gpio_0))
                printf("Right turn\n");

        } else if ((right_wl_edge - left_wl_edge) < -DIST_THRESHOLD){
            if(check_motor_on(axi_gpio_0)){
                XPwm_Set_sw_late_V(&motorL, 25);
                XPwm_Set_sw_late_V(&motorR, 35);
            }
            if(check_debug(axi_gpio_0))
                printf("Left turn\n");

        } else if (abs(right_wl_edge - left_wl_edge) <= DIST_THRESHOLD){
            if(check_motor_on(axi_gpio_0)){
                XPwm_Set_sw_late_V(&motorL, 30);
                XPwm_Set_sw_late_V(&motorR, 30);
            }
            if(check_debug(axi_gpio_0))
                printf("Go straight\n");
        }
        if(check_gabor_LR(axi_gpio_0)){
            bmdc0_axi_lites[0] = (volatile uint32_t)FRAME_BUFFER_ADDRESS + (volatile uint32_t)SVGA_ALL_DISP_ADDRESS; // right
        } else {
            bmdc0_axi_lites[0] = (volatile uint32_t)FRAME_BUFFER_ADDRESS; // left
        }
        if(check_gabor_display(axi_gpio_0)){
            if(check_gabor_LR(axi_gpio_0)){
                gabor_data_disp((uint32_t)FRAME_BUFFER_ADDRESS+(uint32_t)SVGA_ALL_DISP_ADDRESS, (uint32_t)GABOR_RIGHT);
            } else {
                gabor_data_disp((uint32_t)FRAME_BUFFER_ADDRESS, (uint32_t)GABOR_LEFT);
            }
        }
        if(!check_motor_on(axi_gpio_0)){ // sw1 off. Motor Stopped
            XPwm_Set_sw_late_V(&motorL, 0);
            XPwm_Set_sw_late_V(&motorR, 0);
        }
    }
}

ビルドして、wl_tracing_gabor_bm.elf を作成した。
Zybot を使用して実機テストを行った。その様子を示す。


この Zybot の位置で、wl_tracing_gabor_bm.elf を実行した。
sw0 を ON にした時のデバック・メッセージを示す。
Go straight なので、正しそうだ。


この Zybot の位置で、sw2 を OFF した時の左白線用パラメータでのガボール・フィルタ結果を示す。
左白線だけ強調されている。


この Zybot の位置で、sw2 を ON した時の右白線用パラメータでのガボール・フィルタ結果を示す。
右白線だけ強調されている。（写真では左白線も強調されているように見えるが、肉眼ではもっと左白線は薄い）


この Zybot の位置で、sw2 を OFF した時の左白線用パラメータでのガボール・フィルタ数値を BTN0 を押して、取得した。
gabor_left2_230107.txt を示す。


この Zybot の位置で、sw2 を ON した時の右白線用パラメータでのガボール・フィルタ数値を BTN0 を押して、取得した。
gabor_right2_230107.txt を示す。

Zybot でガボール・フィルタを使用して白線間走行する10（作成した IP をブロック・デザインに組み込む）

”Zybot でガボール・フィルタを使用して白線間走行する9（白線コースでデータ収集）”の続き。

Zybot でガボール・フィルタを使用して白線間を走行させたいと思っているということで、前回は、実際のコースでデータを収集したが、家の環境では白線間を走らせることは難しかった。今回は、作ってきた IP を ZYBOt ブロック・デザインに組み込んで、論理合成、インプリメンテーション、ビットストリームの生成を行った。

作ってきた find_startp, Gabr_Filter_lh_3, lap_filter_axis IP 用のディレクトリを ZYBOt_222 ディレクトリに作成した。
作成したディレクトリに各 IP の solution?/impl/export.zip を展開してコピーする。
ZYBOｔ_222 の IP カタログに各 IP を登録した。

ブロック・デザインを開いた。
変更した camera_interface 階層モジュールを示す。


ZYBOｔ ブロック・デザインを示す。



Address Editor 画面を示す。


論理合成、インプリメンテーション、ビットストリームの生成を行って成功した。
Project Summary を示す。