FC2カウンター FPGAの部屋 Zybot でガボール・フィルタを使用して白線間走行する8(BTN0 への変更とソフトウェアの修正)
fc2ブログ

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

FPGAの部屋

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

Zybot でガボール・フィルタを使用して白線間走行する8(BTN0 への変更とソフトウェアの修正)

Zybot でガボール・フィルタを使用して白線間走行する7(Vivado で スイッチ を 4 個実装)”の続き。

Zybot でガボール・フィルタを使用して白線間を走行させたいと思っているということで、前回は、Motor OFF と Debug 表示をスライドスイッチで設定するために axi_gpio_0 を 1 ビット入力から 4 ビット入力に変更した。今回は、この ZYBO 個体の SW4 がおかしいので、BTN0 に変更した。そして、Vivado を論理合成、インプリメンテーション、ビットストリームの生成を行った。タイミング・エラーが発生したが無視することにした。Vitis でアプリケーション・ソフトウェアを変更した。

どうも SW4 の調子がおかしい。具体的には、最初に 0 にしていても 1 に認識されてしまうという不具合があったので、SW4 だけをBTN0 にアサインすることにした。
制約ファイルの ZYBOt.xdc の最後の行を以下のように変更した。

set_property PACKAGE_PIN R18 [get_ports {sw[3]}] # BTN0


Zybot2_45_221223.png

これで、論理合成、インプリメンテーション、ビットストリームの生成を行ったところ、タイミング・エラーが発生した。
Zybot2_46_221223.png

Implementation Design を開いて見たところ、タイミング・エラーはラプラシアン・フィルタ周辺で起きていたので、無視することにした。
Zybot2_47_221223.png

ハードウェアをエクスポートして、Vitis の ZYBOt_wrapper プラットフォームを Update Hardware Specfication した。

アプリケーション・ソフトウェアの wl_tarcking_gabor_bm.c も SW や BTN0 で機能を追加するために変更した。
SW と BTN0 の機能を示す。

sw0 - 0 : NORMAL, 1 : DEBUG
sw1 - 0 : MOTOR OFF, 1 : MOTOR ON
sw2 - 0 : 左白線用のパラメータでガボール・フィルタさせた画像をディスプレイに表示する
sw2 - 1 : 右白線用のパラメータでガボール・フィルタさせた画像をディスプレイに表示する
BTN0 - 1 : sw2 で示された SVGA 画面分のガボール・フィルタの値を標準出力に出力する


Zybot2_38_221223.png

現在の wl_tarcking_gabor_bm.c を貼っておく。

// wl_tracking_gabor_bm.c
// 2022/12/18 : by marsee
// 2022/12/21 : bug fix. by marsee
// 2022/12/23 : Added sw2, BTN0.

#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            255
#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();
}

// Gobor filter
//
void gabor_fil_on(){
    volatile uint32_t *axis_switch_0, *axis_switch_1;
    volatile uint32_t *gabor_filter_lh_0;

    axis_switch_0 = (volatile uint32_t *)XPAR_CAMERA_INTERFACE_AXIS_SWITCH_0_BASEADDR;
    axis_switch_1 = (volatile uint32_t *)XPAR_CAMERA_INTERFACE_AXIS_SWITCH_1_BASEADDR;
    gabor_filter_lh_0 = (volatile uint32_t *)XPAR_CAMERA_INTERFACE_GABOR_FILTER_LH_2_0_S_AXI_CONTROL_BASEADDR;

    // 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; axi_switch_0 M00_AXIS disable
    axis_switch_1[17] = 0x80000000; // 0x44 = 0x80000000; axi_switch_0 M01_AXIS disable
    axis_switch_1[18] = 0x80000000; // 0x48 = 0x80000000; axi_switch_0 M02_AXIS disable
    axis_switch_1[19] = 0x80000000; // 0x4C = 0x80000000; axi_switch_0 M03_AXIS disable
    axis_switch_1[20] = 0; // 0x50 = 0; axi_switch_4 M00_AXIS enable. gabor fitler
    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] = 0x4; // 0x40 = 0x4; gabor fitler
    axis_switch_0[0] = 0x2; // 0x0 = 2; Commit registers
}

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

    if(lr == GABOR_LEFT)
        printf("----- gabor left data -----\n");
    else
        printf("----- gabor right data -----\n");
    for(int i=0; i<SVGA_HORIZONTAL_PIXELS*SVGA_VERTICAL_LINES; i++){
        if((i%40) == 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;
    volatile uint32_t *frame_buffer;
    // 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;

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

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

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

    frame_buffer = (volatile uint32_t *)FRAME_BUFFER_ADDRESS;

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

    // IP start
    dmaw4gabor_0[0] = (volatile uint32_t)0x81; // start, auto restart
    gabor_fil_on();
    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){
        // 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);
        }
    }
}


  1. 2022年12月23日 05:05 |
  2. Zybot
  3. | トラックバック:0
  4. | コメント:0

コメント

コメントの投稿


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

トラックバック URL
https://marsee101.blog.fc2.com/tb.php/5801-b52638bb
この記事にトラックバックする(FC2ブログユーザー)