This program was added the mark detection program on the trace program.
Dependencies: GR-PEACH_video mbed
main.cpp
- Committer:
- ssuzukito
- Date:
- 2016-03-02
- Revision:
- 1:551e0f0cd55d
- Parent:
- 0:5db2664ff378
File content as of revision 1:551e0f0cd55d:
//------------------------------------------------------------------// //Supported MCU: RZ/A1H //File Contents: Trace program 2 (Mark detection) by image processing // (GR-PEACH version on the Micon Car) //Version number: Ver.1.01 //Date: 2016.02.23 //Copyright: Renesas Electronics Corporation //------------------------------------------------------------------// //This program supports the following boards: //* GR-PEACH(E version) //* Motor drive board Ver.5 //* Camera module (SC-310) //Include //------------------------------------------------------------------// #include "mbed.h" #include "math.h" #include "iodefine.h" #include "DisplayBace.h" //Define //------------------------------------------------------------------// //Motor PWM cycle #define MOTOR_PWM_CYCLE 33332 /* Motor PWM period */ /* 1ms P0φ/1 = 0.03us */ //Motor speed #define MAX_SPEED 30 /* motor() set: 0 to 100 */ //Servo PWM cycle #define SERVO_PWM_CYCLE 33332 /* SERVO PWM period */ /* 16ms P0φ/16 = 0.48us */ #define SERVO_CENTER 3124 /* 1.5ms / 0.48us - 1 = 3124*/ #define HANDLE_STEP 18 /* 1 degree value */ //Image sensor #define IMAGE_LINE 160 /* Y Line No */ #define IM_GAP_SET 48 /* 160pix - 112pix = 48 */ //Handle #define ANLOG_STEP 60 /* */ //LED Color on GR-PEACH #define LED_OFF 0x00 #define LED_RED 0x01 #define LED_GREEN 0x02 #define LED_YELLOW 0x03 #define LED_BLUE 0x04 #define LED_PURPLE 0x05 #define LED_SKYBLUE 0x06 #define LED_WHITE 0x07 //Status #define RUN 0x00 #define SENSOR 0x01 #define MARK 0x02 #define STOP 0x03 #define ERROR 0xff //Define(NTSC-Video) //------------------------------------------------------------------// #define VIDEO_INPUT_CH (DisplayBase::VIDEO_INPUT_CHANNEL_0) #define VIDEO_INT_TYPE (DisplayBase::INT_TYPE_S0_VFIELD) #define DATA_SIZE_PER_PIC (2u) /*! Frame buffer stride: Frame buffer stride should be set to a multiple of 32 or 128 in accordance with the frame buffer burst transfer mode. */ #define PIXEL_HW (320u) /* QVGA */ #define PIXEL_VW (240u) /* QVGA */ #define VIDEO_BUFFER_STRIDE (((PIXEL_HW * DATA_SIZE_PER_PIC) + 31u) & ~31u) #define VIDEO_BUFFER_HEIGHT (PIXEL_VW) //Constructor //------------------------------------------------------------------// Ticker interrput; Serial pc(USBTX, USBRX); DigitalOut LED_R(P6_13); /* LED1 on the GR-PEACH board */ DigitalOut LED_G(P6_14); /* LED2 on the GR-PEACH board */ DigitalOut LED_B(P6_15); /* LED3 on the GR-PEACH board */ DigitalIn user_botton(P6_0); /* SW1 on the GR-PEACH board */ DigitalOut Left_motor_signal(P4_6); /* Used by motor fanction */ DigitalOut Right_motor_signal(P4_7); /* Used by motor fanction */ DigitalIn push_sw(P2_13); /* SW1 on the Motor Drive board */ DigitalOut LED_3(P2_14); /* LED3 on the Motor Drive board */ DigitalOut LED_2(P2_15); /* LED2 on the Motor Drive board */ //Prototype //------------------------------------------------------------------// void init_MTU2_PWM_Motor( void ); /* Initialize PWM functions */ void init_MTU2_PWM_Servo( void ); /* Initialize PWM functions */ void intTimer( void ); /* Interrupt fanction */ void led_rgb(int led); unsigned int user_button_get( void ); void led_status_process( void ); /* Function for only interrupt */ void led_status_set( int set ); void led_out(int led); unsigned int pushsw_get( void ); void motor( int accele_l, int accele_r ); void handle( int angle ); int diff( int pwm ); void ServoControl_process( void ); //Prototype(NTSC-video) //------------------------------------------------------------------// static void IntCallbackFunc_Vfield(DisplayBase::int_type_t int_type); static void WaitVfield(const int32_t wait_count); static void IntCallbackFunc_Vsync(DisplayBase::int_type_t int_type); static void WaitVsync(const int32_t wait_count); //Prototype(Display Debug) //------------------------------------------------------------------// void ImageData_Serial_Out( unsigned char *Data_Y, int Width ); void ImageData_Serial_Out2( unsigned char *Data_Y, int Width ); void ImageData_Serial_Out3( void ); //Prototype(Trace by Image Processing) //------------------------------------------------------------------// void change_framebuffer_process( void ); int center_line_corrective( void ); void digital_sensor_corrective( void ); void image_SensorAnalog_process( int timer33 ); int image_sensorAnalog_get( void ); unsigned char image_digital_sensor( void ); //Prototype(Mark detection) //------------------------------------------------------------------// void Image_Extraction( unsigned char *Data_Y ); void Image_part_Extraction( unsigned char *Binary, int Width, int Xpix, int Ypix, unsigned char *Data_B, int x_size, int y_size ); void Image_Compression( unsigned char *Data_Y, int Data_W , unsigned char *Comp_Y, int Comp_W ); void Image_Compression2( unsigned char *Data_Y, int Data_W , unsigned char *Comp_Y, int Comp_M ); void Binarization_process( unsigned char *Comp_Y, unsigned char *Binary, long items ); double Standard_Deviation( unsigned char *data, double *Devi, int items ); double Covariance( double *Devi_A, double *Devi_B, int items ); int Judgement_ImageMatching( double covari, double SDevi_A, double SDevi_B ); //Globle //------------------------------------------------------------------// volatile unsigned long cnt0; /* Used by timer function */ volatile unsigned long cnt1; /* Used within main */ volatile int pattern; /* Pattern numbers */ volatile int status_set; /* Status */ volatile int handle_buff; volatile int iServo; const int revolution_difference[] = { 100, 98, 97, 95, 93, 92, 90, 88, 87, 85, 84, 82, 81, 79, 78, 76, 75, 73, 72, 71, 69, 68, 66, 65, 64, 62, 61, 59, 58, 57, 55, 54, 52, 51, 50, 48, 47, 45, 44, 42, 41, 39, 38, 36, 35, 33 }; /* Trace by image processing */ volatile int im_offset_x; volatile int digital_sensor_threshold; volatile int sensor_x[5]; volatile int sensor_mode; volatile int Left_sensor; volatile int Right_sensor; volatile int white; volatile int black; /* Mark detection */ unsigned char ImageData[320*240]; unsigned char ImageComp[160*120]; unsigned char ImageBinary[160*120]; double TempDevi_A[15]; unsigned char TempBinary_A[15] = {0,1,1,1,0, 0,0,1,0,0, 0,0,0,0,0}; double NowDevi[15]; unsigned char NowImageBinary[15]; volatile double retDevi_A; volatile double retDevi_B; volatile double retCovari; volatile int retJudgeIM; volatile int retJudgeIM_Max; int X_buff, Y_buff; //Globle(NTSC-video) //------------------------------------------------------------------// static uint8_t FrameBuffer_Video_A[VIDEO_BUFFER_STRIDE * VIDEO_BUFFER_HEIGHT]__attribute((section("NC_BSS"),aligned(16))); //16 bytes aligned!; static uint8_t FrameBuffer_Video_B[VIDEO_BUFFER_STRIDE * VIDEO_BUFFER_HEIGHT]__attribute((section("NC_BSS"),aligned(16))); //16 bytes aligned!; static volatile int32_t vsync_count; static volatile int32_t vfield_count; uint8_t * write_buff_addr = FrameBuffer_Video_A; uint8_t * save_buff_addr = FrameBuffer_Video_B; //Main //------------------------------------------------------------------// int main( void ) { int i; /* NTSC-Video */ DisplayBase::graphics_error_t error; /* Create DisplayBase object */ DisplayBase Display; /* Graphics initialization process */ error = Display.Graphics_init(NULL); if (error != DisplayBase::GRAPHICS_OK) { printf("Line %d, error %d\n", __LINE__, error); while (1); } error = Display.Graphics_Video_init( DisplayBase::INPUT_SEL_VDEC, NULL); if( error != DisplayBase::GRAPHICS_OK ) { while(1); } /* Interrupt callback function setting (Vsync signal input to scaler 0) */ error = Display.Graphics_Irq_Handler_Set(DisplayBase::INT_TYPE_S0_VI_VSYNC, 0, IntCallbackFunc_Vsync); if (error != DisplayBase::GRAPHICS_OK) { printf("Line %d, error %d\n", __LINE__, error); while (1); } /* Video capture setting (progressive form fixed) */ error = Display.Video_Write_Setting( VIDEO_INPUT_CH, DisplayBase::COL_SYS_NTSC_358, write_buff_addr, VIDEO_BUFFER_STRIDE, DisplayBase::VIDEO_FORMAT_YCBCR422, DisplayBase::WR_RD_WRSWA_32_16BIT, PIXEL_VW, PIXEL_HW ); if (error != DisplayBase::GRAPHICS_OK) { printf("Line %d, error %d\n", __LINE__, error); while (1); } /* Interrupt callback function setting (Field end signal for recording function in scaler 0) */ error = Display.Graphics_Irq_Handler_Set(VIDEO_INT_TYPE, 0, IntCallbackFunc_Vfield); if (error != DisplayBase::GRAPHICS_OK) { printf("Line %d, error %d\n", __LINE__, error); while (1); } /* Video write process start */ error = Display.Video_Start (VIDEO_INPUT_CH); if (error != DisplayBase::GRAPHICS_OK) { printf("Line %d, error %d\n", __LINE__, error); while (1); } /* Video write process stop */ error = Display.Video_Stop (VIDEO_INPUT_CH); if (error != DisplayBase::GRAPHICS_OK) { printf("Line %d, error %d\n", __LINE__, error); while (1); } /* Video write process start */ error = Display.Video_Start (VIDEO_INPUT_CH); if (error != DisplayBase::GRAPHICS_OK) { printf("Line %d, error %d\n", __LINE__, error); while (1); } /* Wait vsync to update resister */ WaitVsync(1); /* Wait 2 Vfield(Top or bottom field) */ WaitVfield(2); /* Initialize MCU functions */ init_MTU2_PWM_Motor(); init_MTU2_PWM_Servo(); interrput.attach(&intTimer, 0.001); pc.baud(230400); /* Initialize Micon Car state */ led_out( 0x0 ); handle( 0 ); motor( 0, 0 ); /* wait to stabilize NTSC signal (about 170ms) */ wait(0.2); led_status_set( SENSOR ); while(1) { sensor_mode = 1; i = center_line_corrective(); pc.printf( "x = %04d, cnt1 = %03d iret = %01d \r", im_offset_x, cnt1, i ); if( i == -1 ) { /* End process */ pattern = 99; break; } else if ( i == 1 ) { sensor_mode = 0; pattern = 0; led_status_set( RUN ); break; } } pc.printf( "\n\r" ); digital_sensor_corrective(); pc.printf( "B = %04d, W = %04d \n\r", black, white ); pc.printf( "\n\r" ); pc.printf( "threshold = %03d \n\r", digital_sensor_threshold ); pc.printf( "\n\r" ); pc.printf( "\n\r" ); pc.printf( "image_sensor_value\n\r" ); while(1) { i = image_digital_sensor(); pc.printf( "= %05d, = 0x%02x, = %03d J= %3d%% X=%2d Y=%2d DIGI=%x\r", image_sensorAnalog_get(), i, handle_buff, retJudgeIM_Max, X_buff, Y_buff, image_digital_sensor() ); switch( pattern ) { /***************************************************************** About patern 0:wait for switch input 1:check if start bar is open 11:normal trace *****************************************************************/ case 0: /* wait for switch input */ handle( iServo ); if( pushsw_get() ) { led_out( 0x0 ); led_status_set( RUN ); pattern = 11; cnt1 = 0; break; } if( cnt1 < 100 ) { led_out( 0x1 ); } else if( cnt1 < 200 ) { led_out( 0x2 ); } else { cnt1 = 0; } break; case 11: /* normal trace */ handle( iServo ); if( retJudgeIM_Max >= 85 ) { pattern = 90; break; } if( handle_buff > 5 ) { motor( 100, diff(100) ); } else if( handle_buff < -5 ) { motor( diff(100), 100 ); } else { motor( 100, 100 ); } break; case 90: led_status_set( MARK ); motor( 0, 0 ); break; case 99: led_status_set( ERROR ); motor( 0, 0 ); break; default: break; } } } //Initialize MTU2 PWM functions //------------------------------------------------------------------// //MTU2_3, MTU2_4 //Reset-Synchronized PWM mode //TIOC4A(P4_4) :Left-motor //TIOC4B(P4_5) :Right-motor //------------------------------------------------------------------// void init_MTU2_PWM_Motor( void ) { /* Port setting for S/W I/O Contorol */ /* alternative mode */ /* MTU2_4 (P4_4)(P4_5) */ GPIOPBDC4 = 0x0000; /* Bidirection mode disabled*/ GPIOPFCAE4 &= 0xffcf; /* The alternative function of a pin */ GPIOPFCE4 |= 0x0030; /* The alternative function of a pin */ GPIOPFC4 &= 0xffcf; /* The alternative function of a pin */ /* 2nd altemative function/output */ GPIOP4 &= 0xffcf; /* */ GPIOPM4 &= 0xffcf; /* p4_4,P4_5:output */ GPIOPMC4 |= 0x0030; /* P4_4,P4_5:double */ /* Mosule stop 33(MTU2) canceling */ CPGSTBCR3 &= 0xf7; /* MTU2_3 and MTU2_4 (Motor PWM) */ MTU2TCR_3 = 0x20; /* TCNT Clear(TGRA), P0φ/1 */ MTU2TOCR1 = 0x04; /* */ MTU2TOCR2 = 0x40; /* N L>H P H>L */ MTU2TMDR_3 = 0x38; /* Buff:ON Reset-Synchronized PWM mode */ MTU2TMDR_4 = 0x30; /* Buff:ON */ MTU2TOER = 0xc6; /* TIOC3B,4A,4B enabled output */ MTU2TCNT_3 = MTU2TCNT_4 = 0; /* TCNT3,TCNT4 Set 0 */ MTU2TGRA_3 = MTU2TGRC_3 = MOTOR_PWM_CYCLE; /* PWM-Cycle(1ms) */ MTU2TGRA_4 = MTU2TGRC_4 = 0; /* Left-motor(P4_4) */ MTU2TGRB_4 = MTU2TGRD_4 = 0; /* Right-motor(P4_5) */ MTU2TSTR |= 0x40; /* TCNT_4 Start */ } //Initialize MTU2 PWM functions //------------------------------------------------------------------// //MTU2_0 //PWM mode 1 //TIOC0A(P4_0) :Servo-motor //------------------------------------------------------------------// void init_MTU2_PWM_Servo( void ) { /* Port setting for S/W I/O Contorol */ /* alternative mode */ /* MTU2_0 (P4_0) */ GPIOPBDC4 = 0x0000; /* Bidirection mode disabled*/ GPIOPFCAE4 &= 0xfffe; /* The alternative function of a pin */ GPIOPFCE4 &= 0xfffe; /* The alternative function of a pin */ GPIOPFC4 |= 0x0001; /* The alternative function of a pin */ /* 2nd alternative function/output */ GPIOP4 &= 0xfffe; /* */ GPIOPM4 &= 0xfffe; /* p4_0:output */ GPIOPMC4 |= 0x0001; /* P4_0:double */ /* Mosule stop 33(MTU2) canceling */ CPGSTBCR3 &= 0xf7; /* MTU2_0 (Motor PWM) */ MTU2TCR_0 = 0x22; /* TCNT Clear(TGRA), P0φ/16 */ MTU2TIORH_0 = 0x52; /* TGRA L>H, TGRB H>L */ MTU2TMDR_0 = 0x32; /* TGRC and TGRD = Buff-mode*/ /* PWM-mode1 */ MTU2TCNT_0 = 0; /* TCNT0 Set 0 */ MTU2TGRA_0 = MTU2TGRC_0 = SERVO_PWM_CYCLE; /* PWM-Cycle(16ms) */ MTU2TGRB_0 = MTU2TGRD_0 = 0; /* Servo-motor(P4_0) */ MTU2TSTR |= 0x01; /* TCNT_0 Start */ } //Interrupt Timer //------------------------------------------------------------------// void intTimer( void ) { int x, y; static int counter = 0; cnt0++; cnt1++; /* Trace by image processing */ image_SensorAnalog_process( counter ); ServoControl_process(); switch( counter++ ) { case 0: change_framebuffer_process(); break; case 1: Image_Extraction( ImageData ); break; case 2: Image_Extraction( ImageData ); break; case 3: Image_Extraction( ImageData ); break; case 4: Image_Extraction( ImageData ); break; case 5: Image_Extraction( ImageData ); break; case 6: Image_Extraction( ImageData ); break; case 7: Image_Extraction( ImageData ); break; case 8: Image_Extraction( ImageData ); break; case 9: Image_Compression2( ImageData, 320, ImageComp, 16 ); break; case 10: Image_Compression2( ImageData, 320, ImageComp, 16 ); break; case 11: Binarization_process( ImageComp, ImageBinary, 20*15 ); break; case 12: retDevi_A = Standard_Deviation( TempBinary_A, TempDevi_A, 15 ); break; case 13: retJudgeIM_Max = 0; for( y = 0; y <= 12; y++ ) { for( x = 0; x <= 15; x++ ) { Image_part_Extraction( ImageBinary, 20, x, y, NowImageBinary, 5, 3 ); retDevi_B = Standard_Deviation( NowImageBinary, NowDevi, 15 ); retCovari = Covariance( TempDevi_A, NowDevi, 15 ); retJudgeIM = 0; retJudgeIM = Judgement_ImageMatching( retCovari, retDevi_A, retDevi_B ); if( 100 >= retJudgeIM && retJudgeIM > retJudgeIM_Max ) { X_buff = x; Y_buff = y; retJudgeIM_Max = retJudgeIM; } } } break; case 33: counter = 0; break; default: break; } led_status_process(); } //LED_RGB(on GR-PEACH board) //------------------------------------------------------------------// void led_rgb(int led) { LED_R = led & 0x1; LED_G = (led >> 1 ) & 0x1; LED_B = (led >> 2 ) & 0x1; } //user_button_get(on GR-PEACH board) //------------------------------------------------------------------// unsigned int user_button_get( void ) { return (~user_botton) & 0x1; /* Read ports with switches */ } //LED_Status(on GR-PEACH board) Function for only interrupt //------------------------------------------------------------------// void led_status_process( void ) { static unsigned long led_timer; int led_set; int on_time; int off_time; /* setting */ switch( status_set ){ case RUN: led_set = LED_GREEN; on_time = 500; off_time = 500; break; case SENSOR: led_set = LED_BLUE; on_time = 50; off_time = 50; break; case MARK: led_set = LED_RED; on_time = 250; off_time = 250; break; case STOP: led_set = LED_RED; on_time = 1; off_time = 0; break; case ERROR: led_set = LED_RED; on_time = 50; off_time = 50; break; default: led_set = LED_OFF; on_time = 0; off_time = 1; break; } /* Display */ led_timer++; if( led_timer < on_time ) led_rgb( led_set ); else if( led_timer < ( on_time + off_time ) ) led_rgb( LED_OFF ); else led_timer = 0; } //LED_Status(on GR-PEACH board) Function for only interrupt //------------------------------------------------------------------// void led_status_set( int set ) { status_set = set; } //led_out(on Motor drive board) //------------------------------------------------------------------// void led_out(int led) { led = ~led; LED_3 = led & 0x1; LED_2 = ( led >> 1 ) & 0x1; } //pushsw_get(on Motor drive board) //------------------------------------------------------------------// unsigned int pushsw_get( void ) { return (~push_sw) & 0x1; /* Read ports with switches */ } //motor speed control(PWM) //Arguments: motor:-100 to 100 //Here, 0 is stop, 100 is forward, -100 is reverse //------------------------------------------------------------------// void motor( int accele_l, int accele_r ) { accele_l = ( accele_l * MAX_SPEED ) / 100; accele_r = ( accele_r * MAX_SPEED ) / 100; /* Left Motor Control */ if( accele_l >= 0 ) { /* forward */ Left_motor_signal = 0; MTU2TGRC_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * accele_l / 100; } else { /* reverse */ Left_motor_signal = 1; MTU2TGRC_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * ( -accele_l ) / 100; } /* Right Motor Control */ if( accele_r >= 0 ) { /* forward */ Right_motor_signal = 0; MTU2TGRD_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * accele_r / 100; } else { /* reverse */ Right_motor_signal = 1; MTU2TGRD_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * ( -accele_r ) / 100; } } //Handle fanction //------------------------------------------------------------------// void handle( int angle ) { handle_buff = angle; /* When the servo move from left to right in reverse, replace "-" with "+" */ MTU2TGRD_0 = SERVO_CENTER - angle * HANDLE_STEP; } //Deff fanction //------------------------------------------------------------------// int diff( int pwm ) { int i, ret; i = handle_buff; if( i < 0 ) i = -i; if( i > 45 ) i = 45; ret = revolution_difference[i] * pwm / 100; return ret; } //ServoControl_process //------------------------------------------------------------------// void ServoControl_process( void ) { static int iSensorPattern = 0; /* -3° ~ +3° */ iServo = image_sensorAnalog_get() / ANLOG_STEP; if( !sensor_mode ) { switch( iSensorPattern ) { case 0: switch( image_digital_sensor()&0x0f ) { case 0x03: iServo = 14; break; case 0x01: iServo = 20; iSensorPattern = 11; break; case 0x0c: iServo = -14; break; case 0x08: iServo = -20; iSensorPattern = 12; break; default: break; } break; case 11: /* Left side */ iServo = 23; if( (image_digital_sensor()&0x0f) == 0x03 ) { iSensorPattern = 0; } break; case 12: /* right side */ iServo = -23; if( (image_digital_sensor()&0x0f) == 0x0c ) { iSensorPattern = 0; } break; } } } //Image Data Output( for the Excel ) //------------------------------------------------------------------// void ImageData_Serial_Out( unsigned char *Data_Y, int Width ) { int Xp, Yp, inc, Height; Height = (Width / (double)4) * 3; for( Yp = 0, inc = 0; Yp < Height; Yp++ ) { for( Xp = 0; Xp < Width; Xp++, inc++ ) { pc.printf( "%d,", Data_Y[ inc ] ); } pc.printf("\n\r"); } } //Image Data Output2( for TeraTerm ) //------------------------------------------------------------------// void ImageData_Serial_Out2( unsigned char *Data_Y, int Width ) { int Xp, Yp, Height; Height = (Width / (double)4) * 3; for( Yp = 0; Yp < Height; Yp++ ) { for( Xp = 0; Xp < Width; Xp++ ) { pc.printf( "%d ", Data_Y[Xp + (Yp * Width)] ); } pc.printf( "\n\r" ); } pc.printf( "\033[%dA" , Height ); } //Image Data Output3( for the converter ( csv --> jpg ) ) //------------------------------------------------------------------// void ImageData_Serial_Out3( void ) { int Xp, Yp, x, y; /* Camera module test process */ pc.printf( "//,X-Size,Y-Size" ); pc.printf( "\n\r" ); pc.printf( "#SIZE,320,240" ); pc.printf( "\n\r" ); pc.printf( "//,X-Point,Y-Point" ); pc.printf( "\n\r" ); for( Yp = 0, y = 0; Yp < 240; Yp+=1, y++ ){ for( Xp = 0, x = 0; Xp < 640; Xp+=4, x+=2 ){ pc.printf( "#YCbCr," ); /*Xp*/pc.printf( "%d,", x); /*Yp*/pc.printf( "%d,", y); /*Y0*/pc.printf( "%d,", save_buff_addr[(Xp+0)+(640*Yp)]);//6 /*Cb*/pc.printf( "%d,", save_buff_addr[(Xp+1)+(640*Yp)]);//5 /*Cr*/pc.printf( "%d,", save_buff_addr[(Xp+3)+(640*Yp)]);//7 pc.printf( "\n\r" ); pc.printf( "#YCbCr," ); /*Xp*/pc.printf( "%d,", x+1); /*Yp*/pc.printf( "%d,", y); /*Y1*/pc.printf( "%d,", save_buff_addr[(Xp+2)+(640*Yp)]);//4 /*Cb*/pc.printf( "%d,", save_buff_addr[(Xp+1)+(640*Yp)]);//5 /*Cr*/pc.printf( "%d,", save_buff_addr[(Xp+3)+(640*Yp)]);//7 pc.printf( "\n\r" ); } } } //Change FrameBuffer Process //------------------------------------------------------------------// void change_framebuffer_process( void ) { DisplayBase::graphics_error_t error; DisplayBase Display; /* Change address buffer */ if (write_buff_addr == FrameBuffer_Video_A) { write_buff_addr = FrameBuffer_Video_B; save_buff_addr = FrameBuffer_Video_A; } else { write_buff_addr = FrameBuffer_Video_A; save_buff_addr = FrameBuffer_Video_B; } /* Change write buffer */ error = Display.Video_Write_Change( VIDEO_INPUT_CH, write_buff_addr, VIDEO_BUFFER_STRIDE); if (error != DisplayBase::GRAPHICS_OK) { printf("Line %d, error %d\n", __LINE__, error); while (1); } } //center_line_corrective_process //------------------------------------------------------------------// int center_line_corrective( void ) { static int process_No = 0; static int im_cnt = 0; static int value = 0; int iRet = 0; int image_sensor_value; image_sensor_value = image_sensorAnalog_get(); switch( process_No ) { case 0: /* center_line_corrective */ if( (value > image_sensor_value) && (image_sensor_value > -value) ) { cnt1 = 0; process_No = 1; } if( cnt1 >= 34 ) process_No = 3; break; case 1: /* Retrial after 100ms */ if( cnt1 >= 68 ) { if( (value > image_sensor_value) && (image_sensor_value > -value) ) { cnt1 = 0; process_No = 2; } else { process_No = 3; } } break; case 2: iRet = 1; break; case 3: im_offset_x += 2; if( im_offset_x > ( ((160 - IM_GAP_SET) + (IM_GAP_SET * 2)) * 2 ) ) { im_offset_x = 160 - IM_GAP_SET; im_cnt++; } if( im_cnt >= 1 ) { im_cnt = 0; value += 2; if( value >= 6 ) { value = 6; iRet = -1; } } cnt1 = 0; process_No = 0; break; default: break; } return iRet; } //digital_sensor_corrective //------------------------------------------------------------------// void digital_sensor_corrective( void ) { int Ypix; int Xpix; int pix_value; black = 127; white = 127; /* Maximum value of the black and white */ Ypix = IMAGE_LINE; for( Xpix = (im_offset_x - 192); Xpix < (im_offset_x + 192); Xpix+=4 ) { pix_value = save_buff_addr[(Ypix * 640) + (Xpix + 0)];/* Y1 */ if( black >= pix_value ) black = pix_value; if( white <= pix_value ) white = pix_value; pix_value = save_buff_addr[(Ypix * 640) + (Xpix + 2)];/* Y2 */ if( black >= pix_value ) black = pix_value; if( white <= pix_value ) white = pix_value; } /* threshold value */ digital_sensor_threshold = ( white - black ) / 2; /* X3 */ Ypix = IMAGE_LINE; for( Xpix = (im_offset_x - 192); Xpix < im_offset_x; Xpix+=4 ) { pix_value = save_buff_addr[(Ypix * 640) + (Xpix + 0)];/* Y1 */ pix_value += save_buff_addr[(Ypix * 640) + (Xpix + 2)];/* Y2 */ pix_value /= 2; if( pix_value > digital_sensor_threshold ) { sensor_x[3] = Xpix; break; } } /* X2 */ sensor_x[2] = im_offset_x; /* X1 */ Ypix = IMAGE_LINE; for( Xpix = im_offset_x; Xpix < (im_offset_x + 192); Xpix+=4 ) { pix_value = save_buff_addr[(Ypix * 640) + (Xpix + 0)];/* Y1 */ pix_value += save_buff_addr[(Ypix * 640) + (Xpix + 2)];/* Y2 */ pix_value /= 2; if( pix_value < digital_sensor_threshold ) { sensor_x[1] = Xpix; break; } } /* X4 */ sensor_x[4] = sensor_x[3] - (sensor_x[2] - sensor_x[3]); /* X0 */ sensor_x[0] = (sensor_x[1] - sensor_x[2]) + sensor_x[1]; pc.printf( "x4 = %03d, x3 = %03d, x2 = %03d, x1 = %03d, x0 = %03d \n\n\r", sensor_x[4], sensor_x[3], sensor_x[2], sensor_x[1], sensor_x[0] ); } //image_SensorAnalog_process //------------------------------------------------------------------// void image_SensorAnalog_process( int timer33 ) { int Ypix; int Xpix; int left_value; int right_value; /* Initialization */ left_value = 0; right_value = 0; /* left image value ( 64pix to 160pix ) */ Ypix = IMAGE_LINE - (timer33);//109 to 75 line for( Xpix = (im_offset_x - 192); Xpix < im_offset_x; Xpix+=4 ) { left_value += save_buff_addr[(Ypix * 640) + (Xpix + 0)];/* Y1 */ left_value += save_buff_addr[(Ypix * 640) + (Xpix + 2)];/* Y2 */ } /* right image value ( 161pix to 256pix ) */ Ypix = IMAGE_LINE - (timer33);//109 to 75 line for( Xpix = im_offset_x; Xpix < ( im_offset_x + 192 ); Xpix+=4 ) { right_value += save_buff_addr[(Ypix * 640) + (Xpix + 0)];/* Y1 */ right_value += save_buff_addr[(Ypix * 640) + (Xpix + 2)];/* Y2 */ } Left_sensor = left_value / 10; Right_sensor = right_value / 10; } //image_sensorAnalog_get //------------------------------------------------------------------// int image_sensorAnalog_get( void ) { int iRet; iRet = Left_sensor - Right_sensor; /* Reverse of the positive and negative. */ iRet *= -1; if( iRet <= -900 ) iRet = -900;//Left side if( iRet >= 900 ) iRet = 900;//right side return iRet; } //image_digital_sensor //------------------------------------------------------------------// unsigned char image_digital_sensor( void ) { int Ypix; int Xpix; int data; unsigned char sensor; sensor = 0; /* sensor 2 (center) */ data = 0; Ypix = IMAGE_LINE; for( Xpix = (sensor_x[2] - 8); Xpix < (sensor_x[2] + 8); Xpix+=4 ) { data += save_buff_addr[(Ypix * 640) + (Xpix + 0)];/* Y1 */ data += save_buff_addr[(Ypix * 640) + (Xpix + 2)];/* Y2 */ } data /= 8; if( data <= digital_sensor_threshold ) data = 0; else data = 1; sensor |= (data << 4) & 0x10; /* sensor 4 (left side) */ data = 0; Ypix = IMAGE_LINE; for( Xpix = (sensor_x[4] - 16); Xpix < sensor_x[4]; Xpix+=4 ) { data += save_buff_addr[(Ypix * 640) + (Xpix + 0)];/* Y1 */ data += save_buff_addr[(Ypix * 640) + (Xpix + 2)];/* Y2 */ } data /= 8; if( data <= digital_sensor_threshold ) data = 0; else data = 1; sensor |= (data << 3) & 0x08; /* sensor 3 (left side) */ data = 0; Ypix = IMAGE_LINE; for( Xpix = (sensor_x[3] - 16); Xpix < sensor_x[3]; Xpix+=4 ) { data += save_buff_addr[(Ypix * 640) + (Xpix + 0)];/* Y1 */ data += save_buff_addr[(Ypix * 640) + (Xpix + 2)];/* Y2 */ } data /= 8; if( data <= digital_sensor_threshold ) data = 0; else data = 1; sensor |= (data << 2) & 0x04; /* sensor 1 (right side) */ data = 0; Ypix = IMAGE_LINE; for( Xpix = sensor_x[1]; Xpix < (sensor_x[1] + 16); Xpix+=4 ) { data += save_buff_addr[(Ypix * 640) + (Xpix + 0)];/* Y1 */ data += save_buff_addr[(Ypix * 640) + (Xpix + 2)];/* Y2 */ } data /= 8; if( data <= digital_sensor_threshold ) data = 0; else data = 1; sensor |= (data << 1) & 0x02; /* sensor 0 (right side) */ data = 0; Ypix = IMAGE_LINE; for( Xpix = sensor_x[0]; Xpix < (sensor_x[0] + 16); Xpix+=4 ) { data += save_buff_addr[(Ypix * 640) + (Xpix + 0)];/* Y1 */ data += save_buff_addr[(Ypix * 640) + (Xpix + 2)];/* Y2 */ } data /= 8; if( data <= digital_sensor_threshold ) data = 0; else data = 1; sensor |= (data) & 0x01; sensor &= 0x1f; return sensor; } //Image Data YCbCr -> Y(320*240pix) //------------------------------------------------------------------// void Image_Extraction( unsigned char *Data_Y ) { static int Xp, Yp, inc; static int counter = 0; #if 0 // Function for( Yp = 0, inc = 0; Yp < 240; Yp++ ){ for( Xp = 0; Xp < 640; Xp+=2, inc++ ){ Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)]; } } #else // Distributed processing switch( counter++ ) { case 0: for( Yp = 0, inc = 0; Yp < 30; Yp++ ){ for( Xp = 0; Xp < 640; Xp+=2, inc++ ){ Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)]; } } break; case 1: for( ; Yp < 60; Yp++ ){ for( Xp = 0; Xp < 640; Xp+=2, inc++ ){ Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)]; } } break; case 2: for( ; Yp < 90; Yp++ ){ for( Xp = 0; Xp < 640; Xp+=2, inc++ ){ Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)]; } } break; case 3: for( ; Yp < 120; Yp++ ){ for( Xp = 0; Xp < 640; Xp+=2, inc++ ){ Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)]; } } break; case 4: for( ; Yp < 150; Yp++ ){ for( Xp = 0; Xp < 640; Xp+=2, inc++ ){ Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)]; } } break; case 5: for( ; Yp < 180; Yp++ ){ for( Xp = 0; Xp < 640; Xp+=2, inc++ ){ Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)]; } } break; case 6: for( ; Yp < 210; Yp++ ){ for( Xp = 0; Xp < 640; Xp+=2, inc++ ){ Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)]; } } break; case 7: for( ; Yp < 240; Yp++ ){ for( Xp = 0; Xp < 640; Xp+=2, inc++ ){ Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)]; } } counter = 0; break; default: break; } #endif } //Image_Compression Y ( Thinning processing ) //------------------------------------------------------------------// void Image_Compression( unsigned char *Data_Y, int Data_W , unsigned char *Comp_Y, int Comp_W ) { int Data_H, Comp_H, Step_W, Step_H; int Xp, Yp, inc; Data_H = (Data_W / (double)4) * 3; Comp_H = (Comp_W / (double)4) * 3; Step_W = Data_W / Comp_W; Step_H = Data_H / Comp_H; for( Yp = 0, inc = 0; Yp < Comp_H; Yp++ ) { for( Xp = 0; Xp < Comp_W; Xp++, inc++ ) { Comp_Y[inc] = Data_Y[(Yp * (Data_W * Step_H)) + (Xp * Step_W)]; } } } //Image_Compression2 Y ( Averaging processing ) //------------------------------------------------------------------// void Image_Compression2( unsigned char *Data_Y, int Data_W , unsigned char *Comp_Y, int Comp_M ) { int Data_H, Pixel_T, Pixel_D; int x, y; static int Xp, Yp, inc; static int counter = 0; Data_H = (Data_W / (double)4) * 3; Pixel_D = Comp_M * Comp_M; #if 0 // Function for( Yp = 0, inc = 0; Yp < Data_H; Yp+=Comp_M ){ for( Xp = 0; Xp < Data_W; Xp+=Comp_M, inc++ ){ Pixel_T = 0; for( y = 0; y < Comp_M; y++ ){ for( x = 0; x < Comp_M; x++ ){ Pixel_T += Data_Y[( Xp + x ) + (( Yp + y ) * Data_W )]; } } Comp_Y[inc] = Pixel_T / Pixel_D; } } #else // Distributed processing switch( counter++ ) { case 0: for( Yp = 0, inc = 0; Yp < (Data_H / 2); Yp+=Comp_M ){ for( Xp = 0; Xp < Data_W; Xp+=Comp_M, inc++ ){ Pixel_T = 0; for( y = 0; y < Comp_M; y++ ){ for( x = 0; x < Comp_M; x++ ){ Pixel_T += Data_Y[( Xp + x ) + (( Yp + y ) * Data_W )]; } } Comp_Y[inc] = Pixel_T / Pixel_D; } } break; case 1: for( ; Yp < Data_H; Yp+=Comp_M ){ for( Xp = 0; Xp < Data_W; Xp+=Comp_M, inc++ ){ Pixel_T = 0; for( y = 0; y < Comp_M; y++ ){ for( x = 0; x < Comp_M; x++ ){ Pixel_T += Data_Y[( Xp + x ) + (( Yp + y ) * Data_W )]; } } Comp_Y[inc] = Pixel_T / Pixel_D; } } counter = 0; break; default: break; } #endif } // Binarization_process //------------------------------------------------------------------// void Binarization_process( unsigned char *Comp_Y, unsigned char *Binary, long items ) { int i, threshold; threshold = 150; for( i = 0; i < items; i++ ) { if( Comp_Y[i] >= threshold ) Binary[i] = 1; else Binary[i] = 0; } } // Extract_Image //------------------------------------------------------------------// void Image_part_Extraction( unsigned char *Binary, int Width, int Xpix, int Ypix, unsigned char *Data_B, int x_size, int y_size ) { int x, y; for( y = 0; y < y_size; y++ ) { for( x = 0; x < x_size; x++ ) { Data_B[ x + ( y * x_size ) ] = Binary[ (Xpix + x) + ( (Ypix + y) * Width ) ]; } } } // Standard deviation //------------------------------------------------------------------// double Standard_Deviation( unsigned char *data, double *Devi, int items ) { int i; double iRet_A, iRet_C, iRet_D; /* A 合計値 平均化 */ iRet_A = 0; for( i = 0; i < items; i++ ) { iRet_A += data[i]; } iRet_A /= items; /* B 偏差値 */ for( i = 0; i < items; i++ ) { Devi[i] = data[i] - iRet_A; } /* C 分散 */ iRet_C = 0; for( i = 0; i < items; i++ ) { iRet_C += ( Devi[i] * Devi[i] ); } iRet_C /= items; /* D 標準偏差 */ iRet_D = sqrt( iRet_C ); return iRet_D; } // Covariance //------------------------------------------------------------------// double Covariance( double *Devi_A, double *Devi_B, int items ) { int i; double iRet, iRet_buff; iRet = 0; for( i = 0; i < items; i++ ) { iRet_buff = Devi_A[i] * Devi_B[i]; iRet += iRet_buff; } iRet /= items; return iRet; } // Judgement_ImageMatching //------------------------------------------------------------------// int Judgement_ImageMatching( double covari, double SDevi_A, double SDevi_B ) { int iRet; iRet = ( covari * 100 ) / ( SDevi_A * SDevi_B ); return iRet; } //******************************************************************// // @brief Interrupt callback function // @param[in] int_type : VDC5 interrupt type // @retval None //*******************************************************************/ static void IntCallbackFunc_Vfield(DisplayBase::int_type_t int_type) { if (vfield_count > 0) { vfield_count--; } } //******************************************************************// // @brief Wait for the specified number of times Vsync occurs // @param[in] wait_count : Wait count // @retval None //*******************************************************************/ static void WaitVfield(const int32_t wait_count) { vfield_count = wait_count; while (vfield_count > 0) { /* Do nothing */ } } //******************************************************************// // @brief Interrupt callback function for Vsync interruption // @param[in] int_type : VDC5 interrupt type // @retval None //*******************************************************************/ static void IntCallbackFunc_Vsync(DisplayBase::int_type_t int_type) { if (vsync_count > 0) { vsync_count--; } } //******************************************************************// // @brief Wait for the specified number of times Vsync occurs // @param[in] wait_count : Wait count // @retval None //*******************************************************************/ static void WaitVsync(const int32_t wait_count) { vsync_count = wait_count; while (vsync_count > 0) { /* Do nothing */ } } //------------------------------------------------------------------// // End of file //------------------------------------------------------------------//