前回は、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 も作成した。
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, 0xf0, 0x1); // Changed regster map to IFP page 1
cam_i2c_write(mt9d111_axi_iic, 0xba, 0x97, 0x20); // 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)
コメント