This program is line trace program by image processing.
Dependencies: GR-PEACH_video mbed
main.cpp
- Committer:
- TetsuyaKonno
- Date:
- 2016-09-02
- Revision:
- 0:98993ca640e2
File content as of revision 0:98993ca640e2:
//------------------------------------------------------------------// //Supported MCU: RZ/A1H //File Contents: Trace Program by Image Processing // (GR-PEACH version on the Micon Car) //Version number: Ver.1.01 //Date: 2016.07.20 //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 40 /* 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 */ //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_T 0x02 #define MARK_C 0x03 #define MARK_R 0x04 #define MARK_L 0x05 #define STOP 0x06 #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 //------------------------------------------------------------------// //Peripheral functions void init_MTU2_PWM_Motor( void ); /* Initialize PWM functions */ void init_MTU2_PWM_Servo( void ); /* Initialize PWM functions */ void intTimer( void ); /* Interrupt fanction */ //GR-peach board void led_rgb(int led); unsigned int user_button_get( void ); //Motor drive board 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 ); //Interrupt function void led_status_process( void ); /* Function for only interrupt */ void led_status_set( int set ); //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) //------------------------------------------------------------------// int CenterLine_Corrective( unsigned char *Binary ); void change_framebuffer_process( void ); /* Function for only interrupt */ void digital_sensor_process( unsigned char *Binary ); /* Function for only interrupt */ unsigned char digital_sensor( void ); //Prototype(Mark detection) //------------------------------------------------------------------// void Image_Extraction( unsigned char *Data_Y ); 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 ); //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; 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 Sensor_X[8][6]; volatile unsigned char sensor_value; /* Mark detection */ unsigned char ImageData[320*240]; unsigned char ImageComp[160*120]; unsigned char ImageBinary[160*120]; int Xt, Yt; int Xc, Yc; int Xr, Yr; int Xl, Yl; //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 ) { /* 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 ); CenterLine_Corrective( ImageBinary ); if( pushsw_get() ) { while( 1 ){ ImageData_Serial_Out2( ImageBinary, 20 ); } } led_status_set( RUN ); while(1) { 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 */ 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 */ switch( (digital_sensor()&0x0f) ) { case 0x00: handle( 0 ); motor( 100, 100 ); break; case 0x02: handle( 3 ); motor( 100, diff(100) ); break; case 0x03: handle( 12 ); motor( 100, diff(100) ); break; case 0x01: handle( 20 ); motor( 100, diff(100) ); pattern = 12; break; case 0x04: handle( -3 ); motor( diff(100), 100 ); break; case 0x0c: handle( -12 ); motor( diff(100), 100 ); break; case 0x08: handle( -20 ); motor( diff(100), 100 ); pattern = 13; break; default: break; } break; case 12: /* Left side */ if( (digital_sensor()&0x02) == 0x02 ) { pattern = 11; break; } switch( (digital_sensor()&0x0f) ) { case 0x01: handle( 20 ); motor( 100, diff(100) ); break; case 0x00: case 0x08: case 0x0c: handle( 22 ); motor( 100, diff(100) ); break; default: break; } break; case 13: /* right side */ if( (digital_sensor()&0x04) == 0x04 ) { pattern = 11; } switch( (digital_sensor()&0x0f) ) { case 0x08: handle( -20 ); motor( diff(100), 100 ); break; case 0x00: case 0x01: case 0x03: handle( -22 ); motor( diff(100), 100 ); break; default: break; } 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 ) { static int counter = 0; cnt0++; cnt1++; /* Mark Check 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 33: counter = 0; break; default: break; } /* Trace by image processing */ digital_sensor_process( ImageBinary ); /* LED processing */ 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_T: led_set = LED_WHITE; on_time = 250; off_time = 250; break; case MARK_C: led_set = LED_YELLOW; on_time = 250; off_time = 250; break; case MARK_R: led_set = LED_PURPLE; on_time = 250; off_time = 250; break; case MARK_L: led_set = LED_SKYBLUE; 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; } //handle(on Motor drive board) //------------------------------------------------------------------// 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; } //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); } } //CenterLine_Corrective image size 20*15pix //------------------------------------------------------------------// int CenterLine_Corrective( unsigned char *Binary ) { #define L 0 #define R 1 int iRet, offset_X, offset_Y; int Xpix, X; int Ypix; int Pixel_diff[2]; int Error_cnt; int value; /* Center of image */ offset_X = 6; offset_Y = 12; /* corrective of center line */ for( Ypix = 0, Error_cnt = 0; Ypix < (offset_Y - 4); Ypix++ ) { for( value = 0; value < 2; value++ ) { for( Xpix = offset_X; Xpix < (offset_X + 8); Xpix++ ) { /* Lift side */ Pixel_diff[L] = 0; if( Binary[ ( ( offset_Y - Ypix ) * 20 ) + Xpix ] >= 1 ) { for( X = Xpix; X > (Xpix - 4); X-- ) { if( Binary[ ( ( offset_Y - Ypix ) * 20 ) + X ] >= 1 ) { Pixel_diff[L]++; } else { break; } } } else { Pixel_diff[L] = -1; } /* Right side */ Pixel_diff[R] = 0; if( Binary[ ( ( offset_Y - Ypix ) * 20 ) + (Xpix + 1) ] >= 1 ) { for( X = (Xpix + 1); X < ((Xpix + 1) + 4); X++ ) { if( Binary[ ( ( offset_Y - Ypix ) * 20 ) + X ] >= 1 ) { Pixel_diff[R]++; } else { break; } } } else { Pixel_diff[R] = 1; } /* check */ iRet = Pixel_diff[L] - Pixel_diff[R]; if( value >= iRet && iRet >= -value ) { break; } } if( value >= iRet && iRet >= -value ) { /* X coordinate */ Sensor_X[Ypix][2] = Xpix; Sensor_X[Ypix][3] = Xpix + 1; break; } else { Sensor_X[Ypix][2] = Sensor_X[Ypix][3] = -1; Error_cnt++; } } /* Left side sensor */ Sensor_X[Ypix][1] = Sensor_X[Ypix][2] - ( Pixel_diff[L] ); Sensor_X[Ypix][0] = Sensor_X[Ypix][1] - ( Pixel_diff[L] + 1 );//( Sensor_X[Ypix][2] - Sensor_X[Ypix][1] ); /* Right side sensor */ Sensor_X[Ypix][4] = Sensor_X[Ypix][3] + ( Pixel_diff[R] ); Sensor_X[Ypix][5] = Sensor_X[Ypix][4] + ( Pixel_diff[R] + 1 );//( Sensor_X[Ypix][4] - Sensor_X[Ypix][3] ); } return Error_cnt; } //digital_sensor_process //------------------------------------------------------------------// void digital_sensor_process( unsigned char *Binary ) { static int counter = 0; static int Ypix = 0; int offset_Y; unsigned char sensor, data; offset_Y = 12; sensor = 0; // Distributed processing switch( counter++ ) { case 15: case 31: data = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][2] ] & 0x01; data |= Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][3] ] & 0x01; sensor |= (data << 4) & 0x10; data = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][0] ] & 0x01; sensor |= (data << 3) & 0x08; data = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][1] ] & 0x01; sensor |= (data << 2) & 0x04; data = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][4] ] & 0x01; sensor |= (data << 1) & 0x02; data = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][5] ] & 0x01; sensor |= (data << 0) & 0x01; sensor &= 0x1f; sensor_value = sensor; Ypix += 4; break; case 33: counter = 0; Ypix = 0; break; default: break; } } //digital_sensor //------------------------------------------------------------------// unsigned char digital_sensor( void ) { return sensor_value; } //Image Data YCbCr -> Y(320*240pix) //------------------------------------------------------------------// void Image_Extraction( unsigned char *Data_Y ) { static int Xp, Yp, inc; static int counter = 0; // 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; } } //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; // 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; } } // Binarization_process //------------------------------------------------------------------// void Binarization_process( unsigned char *Comp_Y, unsigned char *Binary, long items ) { int i, threshold; threshold = 128; for( i = 0; i < items; i++ ) { if( Comp_Y[i] >= threshold ) Binary[i] = 1; else Binary[i] = 0; } } //******************************************************************// // @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 //------------------------------------------------------------------//