をコメントアウトして、下の行に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
をコメントアウトして、下の行に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
// 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;
}
// 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);
}
}
}
日 | 月 | 火 | 水 | 木 | 金 | 土 |
---|---|---|---|---|---|---|
- | - | - | - | 1 | 2 | 3 |
4 | 5 | 6 | 7 | 8 | 9 | 10 |
11 | 12 | 13 | 14 | 15 | 16 | 17 |
18 | 19 | 20 | 21 | 22 | 23 | 24 |
25 | 26 | 27 | 28 | 29 | 30 | - |