FC2カウンター FPGAの部屋 白線追従走行用畳み込みニューラルネットワーク・システムの製作7(白線間を走行)

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

FPGAの部屋

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

白線追従走行用畳み込みニューラルネットワーク・システムの製作7(白線間を走行)

白線追従走行用畳み込みニューラルネットワーク・システムの製作6(実機で確認2)”の続き。

前回は、SDKで進む方向を表示するアプリケーションソフトを製作して、問題がないことを確かめた。今回は、ZYBO のUbuntu 14.04 上でアプリケーションソフトを作り、白線間を走行させる。

Zybot/wl_tracing_cnn ディレクトリを作成し、wl_tracing_cnn.cpp アプリケーションソフトを作成した。
wl_tracing_cnn ディレクトリには、dmar4resize_gray, resize_gray, straight_conv_nn2_axis2 のドライバを入れて、画像フレーム用のバッファ領域を確保するためにikwzm さんの udmabuf もコンパイル済みの状態で入っている。後、コンパイルするためのMakefile も作成した。
wlt_cnn_208_170922.png

make でコンパイルして wl_tracing_cnn を生成した。

ミニ・ロボットカーを白線間にセットして、udmabuf_insmod を実行してから、 wl_tracing_cnn を起動した。
白線間は走行したのだが、左右に振れてしまって真っ直ぐ走らない感じだ。例えば、左旋回して戻るときに白線間の真ん中あたりに車体がまっすぐになるように右に舵を切る必要がある。それを考えていなかったので左右に振れてしまっているのだと思う。
例えば、右旋回して戻るときに直進が無くて、すぐに左旋回になっているイメージだろうか?
動画を貼っておく。




現在の wl_tracing_cnn.cpp を貼っておく。

// wl_traceing_cnn.cpp
// 2017/09/18 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 "xpwm.h"
#include "xdmar4resize_gray.h"
#include "xresize_gray.h"
#include "xstraight_conv_nn2_axis2.h"

#define CMA_START_ADDRESS 0x17800000
#define VIDEO_BUFFER_START_ADDRESS  0x18000000  // Limit 0x18800000, 800*600*4 = 2MBytes * 2

#define HORIZONTAL_PIXEL    800
#define ALL_CHAR_OF_1LINE   (HORIZONTAL_PIXEL/8)
#define VERTICAL_PIXEL      600
#define ALL_CHAR_OF_ROW     (VERTICAL_PIXEL/8)
#define ALL_DISP_ADDRESS    (HORIZONTAL_PIXEL*VERTICAL_PIXEL*4)
#define ALL_DISP_CHARACTOR  (HORIZONTAL_PIXEL*VERTICAL_PIXEL)

#define DEBUG
//#define MOTOR_OFF

void cam_i2c_init(volatile unsigned *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 unsigned *mt9d111_axi_iic, unsigned int device_addr, unsigned int write_addr, unsigned int 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();
}

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


int main()
{
    int fd0, fd1, fd2, fd3, fd4, fd5, fd6, fd7, fd8, fd9, fd10;
    volatile unsigned *bmdc_axi_lites0, *bmdc_axi_lites1;
    volatile unsigned *dmaw4gabor_0;
    volatile unsigned *axis_switch_0, *axis_switch_1;
    volatile unsigned *mt9d111_inf_axis_0;
    volatile unsigned *mt9d111_axi_iic;
    volatile unsigned *axi_gpio_0;
    volatile unsigned *frame_buffer_bmdc;
    char  attr[1024];
    unsigned long  phys_addr;
    int i;
    XDmar4resize_gray xdmar;
    XResize_gray resg;
    XStraight_conv_nn2_axis2 stcnn;
    XPwm motorL, motorR;
    XPwm *motorLp, *motorRp;
    
    motorLp = &motorL;
    motorRp = &motorR;

    // Bitmap Display Controller 0 AXI4 Lite Slave (UIO6)
    fd6 = open("/dev/uio6", O_RDWR); // bitmap_display_controller 0 axi4 lite
    if (fd6 < 1){
        fprintf(stderr, "/dev/uio6 (bitmap_disp_cntrler_axi_master_0) open error\n");
        exit(-1);
    }
    bmdc_axi_lites0 = (volatile unsigned *)mmap(NULL, 0x10000, PROT_READ|PROT_WRITE, MAP_SHARED, fd6, 0);
    if (!bmdc_axi_lites0){
        fprintf(stderr, "bmdc_axi_lites0 mmap error\n");
        exit(-1);
    }
    
    // Bitmap Display Controller 1 AXI4 Lite Slave (UIO7)
    fd7 = open("/dev/uio7", O_RDWR); // bitmap_display_controller axi4 lite
    if (fd7 < 1){
        fprintf(stderr, "/dev/uio7 (bitmap_disp_cntrler_axi_master_0) open error\n");
        exit(-1);
    }
    bmdc_axi_lites1 = (volatile unsigned *)mmap(NULL, 0x10000, PROT_READ|PROT_WRITE, MAP_SHARED, fd7, 0);
    if (!bmdc_axi_lites1){
        fprintf(stderr, "bmdc_axi_lites1 mmap error\n");
        exit(-1);
    }

    // dmaw4gabor_0 (UIO1)
    fd1 = open("/dev/uio1", O_RDWR); // dmaw4gabor_0 interface AXI4 Lite Slave
    if (fd1 < 1){
        fprintf(stderr, "/dev/uio1 (dmaw4gabor_0) open error\n");
        exit(-1);
    }
    dmaw4gabor_0 = (volatile unsigned *)mmap(NULL, 0x10000, PROT_READ|PROT_WRITE, MAP_SHARED, fd1, 0);
    if (!dmaw4gabor_0){
        fprintf(stderr, "dmaw4gabor_0 mmap error\n");
        exit(-1);
    }
    
    // mt9d111 i2c AXI4 Lite Slave (UIO0)
    fd0 = open("/dev/uio0", O_RDWR); // mt9d111 i2c AXI4 Lite Slave
    if (fd0 < 1){
        fprintf(stderr, "/dev/uio0 (mt9d111_axi_iic) open error\n");
        exit(-1);
    }
    mt9d111_axi_iic = (volatile unsigned *)mmap(NULL, 0x10000, PROT_READ|PROT_WRITE, MAP_SHARED, fd0, 0);
    if (!mt9d111_axi_iic){
        fprintf(stderr, "mt9d111_axi_iic mmap error\n");
        exit(-1);
    }

    // mt9d111 inf axis AXI4 Lite Slave (UIO5)
    fd5 = open("/dev/uio5", O_RDWR); // mt9d111 inf axis AXI4 Lite Slave
    if (fd5 < 1){
        fprintf(stderr, "/dev/uio5 (mt9d111_inf_axis_0) open error\n");
        exit(-1);
    }
    mt9d111_inf_axis_0 = (volatile unsigned *)mmap(NULL, 0x10000, PROT_READ|PROT_WRITE, MAP_SHARED, fd5, 0);
    if (!mt9d111_inf_axis_0){
        fprintf(stderr, "mt9d111_inf_axis_0 mmap error\n");
        exit(-1);
    }

    // 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);
    }
    
    // axi_gpio_0 (UIO8)
    fd8 = open("/dev/uio8", O_RDWR); // axi_gpio_0 interface AXI4 Lite Slave
    if (fd8 < 1){
        fprintf(stderr, "/dev/uio8 (axi_gpio_0) open error\n");
        exit(-1);
    }
    axi_gpio_0 = (volatile unsigned *)mmap(NULL, 0x10000, PROT_READ|PROT_WRITE, MAP_SHARED, fd8, 0);
    if (!axi_gpio_0){
        fprintf(stderr, "axi_gpio_8 mmap error\n");
        exit(-1);
    }
    
    // udmabuf0
    fd9 = open("/dev/udmabuf0", O_RDWR | O_SYNC); // frame_buffer, The chache is disabled. 
    if (fd9 == -1){
        fprintf(stderr, "/dev/udmabuf0 open error\n");
        exit(-1);
    }
    frame_buffer_bmdc = (volatile unsigned *)mmap(NULL, 5760000, PROT_READ|PROT_WRITE, MAP_SHARED, fd9, 0);
    if (!frame_buffer_bmdc){
        fprintf(stderr, "frame_buffer_bmdc mmap error\n");
        exit(-1);
    }

    // axis_switch_1, 1to2 ,Select M00_AXIS
    // Refer to http://marsee101.blog19.fc2.com/blog-entry-3177.html
    axis_switch_1[16] = 0x0// 0x40 = 0
    axis_switch_1[17] = 0x80000000// 0x44 = 0x80000000, disable
    axis_switch_1[18] = 0x80000000// 0x48 = 0x80000000, disable
    axis_switch_1[19] = 0x80000000// 0x4C = 0x80000000, disable
    axis_switch_1[0] = 0x2// Comit registers
    
    // axis_switch_0, 2to1, Select S00_AXIS
    // Refer to http://marsee101.blog19.fc2.com/blog-entry-3177.html
    axis_switch_0[16] = 0x0// 0x40 = 0;
    axis_switch_0[0] = 0x2// Comit registers
    
    // phys_addr of udmabuf0
    fd10 = open("/sys/devices/virtual/udmabuf/udmabuf0/phys_addr", O_RDONLY);
    if (fd10 == -1){
        fprintf(stderr, "/sys/devices/virtual/udmabuf/udmabuf0/phys_addr open error\n");
        exit(-1);
    }
    read(fd10, attr, 1024);
    sscanf(attr, "%lx", &phys_addr);  
    close(fd10);
    printf("phys_addr = %x\n", (int)phys_addr);
    
    // DMAW4Gabor Initialization sequence
    dmaw4gabor_0[6] = (unsigned int)phys_addr; // Data signal of frame_buffer0
    dmaw4gabor_0[8] = (unsigned int)phys_addr+ALL_DISP_ADDRESS; // Data signal of frame_buffer1
    dmaw4gabor_0[0] = 0x1// ap_start = 1
    dmaw4gabor_0[0] = 0x80// auto_restart = 1

    // bitmap display controller settings
    bmdc_axi_lites0[0] = (unsigned int)phys_addr; // Bitmap Display Controller 0 start
    bmdc_axi_lites1[0] = (unsigned int)phys_addr; // Bitmap Display Controller 1 start
    mt9d111_inf_axis_0[0] = (unsigned int)phys_addr; // Camera Interface start (Address is dummy)

    // CMOS Camera initialize, MT9D111
    cam_i2c_init(mt9d111_axi_iic);
    
    cam_i2c_write(mt9d111_axi_iic, 0xba, 0xf00x1);        // Changed regster map to IFP page 1
    cam_i2c_write(mt9d111_axi_iic, 0xba, 0x970x20);   // RGB Mode, RGB565

    mt9d111_inf_axis_0[1] = 0;

    // Initialization of xdmar4resize_gray, xresize_gray, xstraight_conv_nn2_axis2
    if(XDmar4resize_gray_Initialize(&xdmar, "dmar4resize_gray_0") != XST_SUCCESS){
        fprintf(stderr, "dmar4resize_gray_0 open error\n");
        exit(-1);
    }
    if(XResize_gray_Initialize(&resg, "resize_gray_0") != XST_SUCCESS){
        fprintf(stderr, "resize_gray_0 open error\n");
        exit(-1);
    }
    if(XStraight_conv_nn2_axis2_Initialize(&stcnn, "straight_conv_nn2_axis2_0") != XST_SUCCESS){
        fprintf(stderr, "straight_conv_nn2_axis2 open error\n");
        exit(-1);
    }

    XDmar4resize_gray_Set_frame_buffer0(&xdmar ,(unsigned int)phys_addr);
    XDmar4resize_gray_Set_frame_buffer1(&xdmar ,(unsigned int)phys_addr+ALL_DISP_ADDRESS);
    XDmar4resize_gray_Start(&xdmar);
    XDmar4resize_gray_EnableAutoRestart(&xdmar);

    XResize_gray_Start(&resg);
    XResize_gray_EnableAutoRestart(&resg);

    XStraight_conv_nn2_axis2_Start(&stcnn);
    XStraight_conv_nn2_axis2_EnableAutoRestart(&stcnn);

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

    // main loop
    printf("White line Tracking start. \n");
    while(1){
        usleep(10000); // 10 ms Wait

        while(!XStraight_conv_nn2_axis2_Get_outs_V_vld(&stcnn)) ;
        switch((int)XStraight_conv_nn2_axis2_Get_outs_V(&stcnn)){
            case 0 : // left turn
#ifndef MOTOR_OFF
                XPwm_Set_sw_late_V(&motorL, 15);
                XPwm_Set_sw_late_V(&motorR, 25);
#endif
#ifdef DEBUG
                printf("Left turn\n"); fflush(stdout);
#endif
                break;
            case 1 : // straight
#ifndef MOTOR_OFF
                XPwm_Set_sw_late_V(&motorL, 22);
                XPwm_Set_sw_late_V(&motorR, 22);
#endif
#ifdef DEBUG
                printf("Go straight\n"); fflush(stdout);
#endif
                break;
            default : // 2, right turn
#ifndef MOTOR_OFF
                XPwm_Set_sw_late_V(&motorL, 25);
                XPwm_Set_sw_late_V(&motorR, 15);
#endif
#ifdef DEBUG
                printf("Right turn\n"); fflush(stdout);
#endif
        }
    }


    munmap((void *)bmdc_axi_lites0, 0x10000);
    munmap((void *)bmdc_axi_lites1, 0x10000);
    munmap((void *)dmaw4gabor_0, 0x10000);
    munmap((void *)mt9d111_inf_axis_0, 0x10000);
    munmap((void *)mt9d111_axi_iic, 0x10000);
    munmap((void *)axis_switch_0, 0x10000);
    munmap((void *)axis_switch_1, 0x10000);
    munmap((void *)axi_gpio_0, 0x10000);
    munmap((void *)frame_buffer_bmdc, 576000);
    
    close(fd0);
    close(fd1);
    close(fd2);
    close(fd3);
    close(fd4);
    close(fd5);
    close(fd6);
    close(fd7);
    close(fd8);
    close(fd9);
    
    return(0);
}


Makefile を貼っておく。

# Makefile(wl_traceing_cnn)
# Referred to http://www.ie.u-ryukyu.ac.jp/~e085739/c.makefile.tuts.html

PROGRAM = wl_traceing_cnn
OBJS = wl_traceing_cnn.o xpwm_linux.o xpwm.o xdmar4resize_gray_linux.o xdmar4resize_gray.o xresize_gray_linux.o xresize_gray.o xstraight_conv_nn2_axis2_linux.o xstraight_conv_nn2_axis2.o

CC = gcc
CFLAGS = -Wall -O2

.SUFFIXES: .c .o

.PHONY: all

all: wl_traceing_cnn

wl_traceing_cnn: $(OBJS)
    $(CC) -Wall -o $@ $(OBJS)
    
.c.o:
    $(CC) $(CFLAGS) -c $<

    
.PHONY: clean
clean:
    $(RM) $(PROGRAM) $(OBJS)

  1. 2017年09月22日 04:19 |
  2. DNN
  3. | トラックバック:0
  4. | コメント:0

コメント

コメントの投稿


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

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