Tomokazu Suzuki
This program was added the mark detection program on the trace program.
Dependencies: GR-PEACH_video mbed
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 //------------------------------------------------------------------// 00002 //Supported MCU: RZ/A1H 00003 //File Contents: Trace program 2 (Mark detection) by image processing 00004 // (GR-PEACH version on the Micon Car) 00005 //Version number: Ver.1.01 00006 //Date: 2016.02.23 00007 //Copyright: Renesas Electronics Corporation 00008 //------------------------------------------------------------------// 00009 00010 //This program supports the following boards: 00011 //* GR-PEACH(E version) 00012 //* Motor drive board Ver.5 00013 //* Camera module (SC-310) 00014 00015 //Include 00016 //------------------------------------------------------------------// 00017 #include "mbed.h" 00018 #include "math.h" 00019 #include "iodefine.h" 00020 #include "DisplayBace.h" 00021 00022 //Define 00023 //------------------------------------------------------------------// 00024 //Motor PWM cycle 00025 #define MOTOR_PWM_CYCLE 33332 /* Motor PWM period */ 00026 /* 1ms P0φ/1 = 0.03us */ 00027 //Motor speed 00028 #define MAX_SPEED 30 /* motor() set: 0 to 100 */ 00029 00030 //Servo PWM cycle 00031 #define SERVO_PWM_CYCLE 33332 /* SERVO PWM period */ 00032 /* 16ms P0φ/16 = 0.48us */ 00033 #define SERVO_CENTER 3124 /* 1.5ms / 0.48us - 1 = 3124*/ 00034 #define HANDLE_STEP 18 /* 1 degree value */ 00035 00036 //Image sensor 00037 #define IMAGE_LINE 160 /* Y Line No */ 00038 #define IM_GAP_SET 48 /* 160pix - 112pix = 48 */ 00039 00040 //Handle 00041 #define ANLOG_STEP 60 /* */ 00042 00043 //LED Color on GR-PEACH 00044 #define LED_OFF 0x00 00045 #define LED_RED 0x01 00046 #define LED_GREEN 0x02 00047 #define LED_YELLOW 0x03 00048 #define LED_BLUE 0x04 00049 #define LED_PURPLE 0x05 00050 #define LED_SKYBLUE 0x06 00051 #define LED_WHITE 0x07 00052 00053 //Status 00054 #define RUN 0x00 00055 #define SENSOR 0x01 00056 #define MARK 0x02 00057 #define STOP 0x03 00058 #define ERROR 0xff 00059 00060 //Define(NTSC-Video) 00061 //------------------------------------------------------------------// 00062 #define VIDEO_INPUT_CH (DisplayBase::VIDEO_INPUT_CHANNEL_0) 00063 #define VIDEO_INT_TYPE (DisplayBase::INT_TYPE_S0_VFIELD) 00064 #define DATA_SIZE_PER_PIC (2u) 00065 00066 /*! Frame buffer stride: Frame buffer stride should be set to a multiple of 32 or 128 00067 in accordance with the frame buffer burst transfer mode. */ 00068 #define PIXEL_HW (320u) /* QVGA */ 00069 #define PIXEL_VW (240u) /* QVGA */ 00070 #define VIDEO_BUFFER_STRIDE (((PIXEL_HW * DATA_SIZE_PER_PIC) + 31u) & ~31u) 00071 #define VIDEO_BUFFER_HEIGHT (PIXEL_VW) 00072 00073 //Constructor 00074 //------------------------------------------------------------------// 00075 Ticker interrput; 00076 Serial pc(USBTX, USBRX); 00077 DigitalOut LED_R(P6_13); /* LED1 on the GR-PEACH board */ 00078 DigitalOut LED_G(P6_14); /* LED2 on the GR-PEACH board */ 00079 DigitalOut LED_B(P6_15); /* LED3 on the GR-PEACH board */ 00080 DigitalIn user_botton(P6_0); /* SW1 on the GR-PEACH board */ 00081 00082 DigitalOut Left_motor_signal(P4_6); /* Used by motor fanction */ 00083 DigitalOut Right_motor_signal(P4_7); /* Used by motor fanction */ 00084 DigitalIn push_sw(P2_13); /* SW1 on the Motor Drive board */ 00085 DigitalOut LED_3(P2_14); /* LED3 on the Motor Drive board */ 00086 DigitalOut LED_2(P2_15); /* LED2 on the Motor Drive board */ 00087 00088 //Prototype 00089 //------------------------------------------------------------------// 00090 void init_MTU2_PWM_Motor( void ); /* Initialize PWM functions */ 00091 void init_MTU2_PWM_Servo( void ); /* Initialize PWM functions */ 00092 void intTimer( void ); /* Interrupt fanction */ 00093 void led_rgb(int led); 00094 unsigned int user_button_get( void ); 00095 void led_status_process( void ); /* Function for only interrupt */ 00096 void led_status_set( int set ); 00097 00098 void led_out(int led); 00099 unsigned int pushsw_get( void ); 00100 void motor( int accele_l, int accele_r ); 00101 void handle( int angle ); 00102 int diff( int pwm ); 00103 void ServoControl_process( void ); 00104 00105 //Prototype(NTSC-video) 00106 //------------------------------------------------------------------// 00107 static void IntCallbackFunc_Vfield(DisplayBase::int_type_t int_type); 00108 static void WaitVfield(const int32_t wait_count); 00109 static void IntCallbackFunc_Vsync(DisplayBase::int_type_t int_type); 00110 static void WaitVsync(const int32_t wait_count); 00111 00112 //Prototype(Display Debug) 00113 //------------------------------------------------------------------// 00114 void ImageData_Serial_Out( unsigned char *Data_Y, int Width ); 00115 void ImageData_Serial_Out2( unsigned char *Data_Y, int Width ); 00116 void ImageData_Serial_Out3( void ); 00117 00118 //Prototype(Trace by Image Processing) 00119 //------------------------------------------------------------------// 00120 void change_framebuffer_process( void ); 00121 int center_line_corrective( void ); 00122 void digital_sensor_corrective( void ); 00123 void image_SensorAnalog_process( int timer33 ); 00124 int image_sensorAnalog_get( void ); 00125 unsigned char image_digital_sensor( void ); 00126 00127 //Prototype(Mark detection) 00128 //------------------------------------------------------------------// 00129 void Image_Extraction( unsigned char *Data_Y ); 00130 void Image_part_Extraction( unsigned char *Binary, int Width, int Xpix, int Ypix, unsigned char *Data_B, int x_size, int y_size ); 00131 void Image_Compression( unsigned char *Data_Y, int Data_W , unsigned char *Comp_Y, int Comp_W ); 00132 void Image_Compression2( unsigned char *Data_Y, int Data_W , unsigned char *Comp_Y, int Comp_M ); 00133 void Binarization_process( unsigned char *Comp_Y, unsigned char *Binary, long items ); 00134 double Standard_Deviation( unsigned char *data, double *Devi, int items ); 00135 double Covariance( double *Devi_A, double *Devi_B, int items ); 00136 int Judgement_ImageMatching( double covari, double SDevi_A, double SDevi_B ); 00137 00138 //Globle 00139 //------------------------------------------------------------------// 00140 volatile unsigned long cnt0; /* Used by timer function */ 00141 volatile unsigned long cnt1; /* Used within main */ 00142 volatile int pattern; /* Pattern numbers */ 00143 volatile int status_set; /* Status */ 00144 volatile int handle_buff; 00145 volatile int iServo; 00146 00147 const int revolution_difference[] = { 00148 100, 98, 97, 95, 93, 00149 92, 90, 88, 87, 85, 00150 84, 82, 81, 79, 78, 00151 76, 75, 73, 72, 71, 00152 69, 68, 66, 65, 64, 00153 62, 61, 59, 58, 57, 00154 55, 54, 52, 51, 50, 00155 48, 47, 45, 44, 42, 00156 41, 39, 38, 36, 35, 00157 33 }; 00158 00159 /* Trace by image processing */ 00160 volatile int im_offset_x; 00161 volatile int digital_sensor_threshold; 00162 volatile int sensor_x[5]; 00163 volatile int sensor_mode; 00164 volatile int Left_sensor; 00165 volatile int Right_sensor; 00166 volatile int white; 00167 volatile int black; 00168 00169 /* Mark detection */ 00170 unsigned char ImageData[320*240]; 00171 unsigned char ImageComp[160*120]; 00172 unsigned char ImageBinary[160*120]; 00173 00174 double TempDevi_A[15]; 00175 unsigned char TempBinary_A[15] = {0,1,1,1,0, 00176 0,0,1,0,0, 00177 0,0,0,0,0}; 00178 double NowDevi[15]; 00179 unsigned char NowImageBinary[15]; 00180 00181 volatile double retDevi_A; 00182 volatile double retDevi_B; 00183 volatile double retCovari; 00184 volatile int retJudgeIM; 00185 volatile int retJudgeIM_Max; 00186 00187 int X_buff, Y_buff; 00188 00189 //Globle(NTSC-video) 00190 //------------------------------------------------------------------// 00191 static uint8_t FrameBuffer_Video_A[VIDEO_BUFFER_STRIDE * VIDEO_BUFFER_HEIGHT]__attribute((section("NC_BSS"),aligned(16))); //16 bytes aligned!; 00192 static uint8_t FrameBuffer_Video_B[VIDEO_BUFFER_STRIDE * VIDEO_BUFFER_HEIGHT]__attribute((section("NC_BSS"),aligned(16))); //16 bytes aligned!; 00193 static volatile int32_t vsync_count; 00194 static volatile int32_t vfield_count; 00195 uint8_t * write_buff_addr = FrameBuffer_Video_A; 00196 uint8_t * save_buff_addr = FrameBuffer_Video_B; 00197 00198 //Main 00199 //------------------------------------------------------------------// 00200 int main( void ) 00201 { 00202 int i; 00203 00204 /* NTSC-Video */ 00205 DisplayBase::graphics_error_t error; 00206 00207 /* Create DisplayBase object */ 00208 DisplayBase Display; 00209 00210 /* Graphics initialization process */ 00211 error = Display.Graphics_init(NULL); 00212 if (error != DisplayBase::GRAPHICS_OK) { 00213 printf("Line %d, error %d\n", __LINE__, error); 00214 while (1); 00215 } 00216 00217 error = Display.Graphics_Video_init( DisplayBase::INPUT_SEL_VDEC, NULL); 00218 if( error != DisplayBase::GRAPHICS_OK ) { 00219 while(1); 00220 } 00221 00222 /* Interrupt callback function setting (Vsync signal input to scaler 0) */ 00223 error = Display.Graphics_Irq_Handler_Set(DisplayBase::INT_TYPE_S0_VI_VSYNC, 0, IntCallbackFunc_Vsync); 00224 if (error != DisplayBase::GRAPHICS_OK) { 00225 printf("Line %d, error %d\n", __LINE__, error); 00226 while (1); 00227 } 00228 00229 /* Video capture setting (progressive form fixed) */ 00230 error = Display.Video_Write_Setting( 00231 VIDEO_INPUT_CH, 00232 DisplayBase::COL_SYS_NTSC_358, 00233 write_buff_addr, 00234 VIDEO_BUFFER_STRIDE, 00235 DisplayBase::VIDEO_FORMAT_YCBCR422, 00236 DisplayBase::WR_RD_WRSWA_32_16BIT, 00237 PIXEL_VW, 00238 PIXEL_HW 00239 ); 00240 if (error != DisplayBase::GRAPHICS_OK) { 00241 printf("Line %d, error %d\n", __LINE__, error); 00242 while (1); 00243 } 00244 00245 /* Interrupt callback function setting (Field end signal for recording function in scaler 0) */ 00246 error = Display.Graphics_Irq_Handler_Set(VIDEO_INT_TYPE, 0, IntCallbackFunc_Vfield); 00247 if (error != DisplayBase::GRAPHICS_OK) { 00248 printf("Line %d, error %d\n", __LINE__, error); 00249 while (1); 00250 } 00251 00252 /* Video write process start */ 00253 error = Display.Video_Start (VIDEO_INPUT_CH); 00254 if (error != DisplayBase::GRAPHICS_OK) { 00255 printf("Line %d, error %d\n", __LINE__, error); 00256 while (1); 00257 } 00258 00259 /* Video write process stop */ 00260 error = Display.Video_Stop (VIDEO_INPUT_CH); 00261 if (error != DisplayBase::GRAPHICS_OK) { 00262 printf("Line %d, error %d\n", __LINE__, error); 00263 while (1); 00264 } 00265 00266 /* Video write process start */ 00267 error = Display.Video_Start (VIDEO_INPUT_CH); 00268 if (error != DisplayBase::GRAPHICS_OK) { 00269 printf("Line %d, error %d\n", __LINE__, error); 00270 while (1); 00271 } 00272 00273 /* Wait vsync to update resister */ 00274 WaitVsync(1); 00275 00276 /* Wait 2 Vfield(Top or bottom field) */ 00277 WaitVfield(2); 00278 00279 /* Initialize MCU functions */ 00280 init_MTU2_PWM_Motor(); 00281 init_MTU2_PWM_Servo(); 00282 interrput.attach(&intTimer, 0.001); 00283 pc.baud(230400); 00284 00285 /* Initialize Micon Car state */ 00286 led_out( 0x0 ); 00287 handle( 0 ); 00288 motor( 0, 0 ); 00289 00290 /* wait to stabilize NTSC signal (about 170ms) */ 00291 wait(0.2); 00292 00293 led_status_set( SENSOR ); 00294 while(1) { 00295 sensor_mode = 1; 00296 i = center_line_corrective(); 00297 pc.printf( "x = %04d, cnt1 = %03d iret = %01d \r", im_offset_x, cnt1, i ); 00298 if( i == -1 ) { 00299 /* End process */ 00300 pattern = 99; 00301 break; 00302 } else if ( i == 1 ) { 00303 sensor_mode = 0; 00304 pattern = 0; 00305 led_status_set( RUN ); 00306 break; 00307 } 00308 } 00309 pc.printf( "\n\r" ); 00310 00311 digital_sensor_corrective(); 00312 pc.printf( "B = %04d, W = %04d \n\r", black, white ); 00313 pc.printf( "\n\r" ); 00314 pc.printf( "threshold = %03d \n\r", digital_sensor_threshold ); 00315 pc.printf( "\n\r" ); 00316 pc.printf( "\n\r" ); 00317 pc.printf( "image_sensor_value\n\r" ); 00318 00319 while(1) { 00320 i = image_digital_sensor(); 00321 pc.printf( "= %05d, = 0x%02x, = %03d J= %3d%% X=%2d Y=%2d DIGI=%x\r", 00322 image_sensorAnalog_get(), i, handle_buff, retJudgeIM_Max, X_buff, Y_buff, image_digital_sensor() ); 00323 switch( pattern ) { 00324 /***************************************************************** 00325 About patern 00326 0:wait for switch input 00327 1:check if start bar is open 00328 11:normal trace 00329 *****************************************************************/ 00330 case 0: 00331 /* wait for switch input */ 00332 handle( iServo ); 00333 if( pushsw_get() ) { 00334 led_out( 0x0 ); 00335 led_status_set( RUN ); 00336 pattern = 11; 00337 cnt1 = 0; 00338 break; 00339 } 00340 if( cnt1 < 100 ) { 00341 led_out( 0x1 ); 00342 } else if( cnt1 < 200 ) { 00343 led_out( 0x2 ); 00344 } else { 00345 cnt1 = 0; 00346 } 00347 break; 00348 00349 case 11: 00350 /* normal trace */ 00351 handle( iServo ); 00352 if( retJudgeIM_Max >= 85 ) { 00353 pattern = 90; 00354 break; 00355 } 00356 00357 if( handle_buff > 5 ) { 00358 motor( 100, diff(100) ); 00359 } else if( handle_buff < -5 ) { 00360 motor( diff(100), 100 ); 00361 } else { 00362 motor( 100, 100 ); 00363 } 00364 break; 00365 00366 case 90: 00367 led_status_set( MARK ); 00368 motor( 0, 0 ); 00369 break; 00370 00371 case 99: 00372 led_status_set( ERROR ); 00373 motor( 0, 0 ); 00374 break; 00375 default: 00376 break; 00377 } 00378 } 00379 } 00380 00381 //Initialize MTU2 PWM functions 00382 //------------------------------------------------------------------// 00383 //MTU2_3, MTU2_4 00384 //Reset-Synchronized PWM mode 00385 //TIOC4A(P4_4) :Left-motor 00386 //TIOC4B(P4_5) :Right-motor 00387 //------------------------------------------------------------------// 00388 void init_MTU2_PWM_Motor( void ) 00389 { 00390 /* Port setting for S/W I/O Contorol */ 00391 /* alternative mode */ 00392 00393 /* MTU2_4 (P4_4)(P4_5) */ 00394 GPIOPBDC4 = 0x0000; /* Bidirection mode disabled*/ 00395 GPIOPFCAE4 &= 0xffcf; /* The alternative function of a pin */ 00396 GPIOPFCE4 |= 0x0030; /* The alternative function of a pin */ 00397 GPIOPFC4 &= 0xffcf; /* The alternative function of a pin */ 00398 /* 2nd altemative function/output */ 00399 GPIOP4 &= 0xffcf; /* */ 00400 GPIOPM4 &= 0xffcf; /* p4_4,P4_5:output */ 00401 GPIOPMC4 |= 0x0030; /* P4_4,P4_5:double */ 00402 00403 /* Mosule stop 33(MTU2) canceling */ 00404 CPGSTBCR3 &= 0xf7; 00405 00406 /* MTU2_3 and MTU2_4 (Motor PWM) */ 00407 MTU2TCR_3 = 0x20; /* TCNT Clear(TGRA), P0φ/1 */ 00408 MTU2TOCR1 = 0x04; /* */ 00409 MTU2TOCR2 = 0x40; /* N L>H P H>L */ 00410 MTU2TMDR_3 = 0x38; /* Buff:ON Reset-Synchronized PWM mode */ 00411 MTU2TMDR_4 = 0x30; /* Buff:ON */ 00412 MTU2TOER = 0xc6; /* TIOC3B,4A,4B enabled output */ 00413 MTU2TCNT_3 = MTU2TCNT_4 = 0; /* TCNT3,TCNT4 Set 0 */ 00414 MTU2TGRA_3 = MTU2TGRC_3 = MOTOR_PWM_CYCLE; 00415 /* PWM-Cycle(1ms) */ 00416 MTU2TGRA_4 = MTU2TGRC_4 = 0; /* Left-motor(P4_4) */ 00417 MTU2TGRB_4 = MTU2TGRD_4 = 0; /* Right-motor(P4_5) */ 00418 MTU2TSTR |= 0x40; /* TCNT_4 Start */ 00419 } 00420 00421 //Initialize MTU2 PWM functions 00422 //------------------------------------------------------------------// 00423 //MTU2_0 00424 //PWM mode 1 00425 //TIOC0A(P4_0) :Servo-motor 00426 //------------------------------------------------------------------// 00427 void init_MTU2_PWM_Servo( void ) 00428 { 00429 /* Port setting for S/W I/O Contorol */ 00430 /* alternative mode */ 00431 00432 /* MTU2_0 (P4_0) */ 00433 GPIOPBDC4 = 0x0000; /* Bidirection mode disabled*/ 00434 GPIOPFCAE4 &= 0xfffe; /* The alternative function of a pin */ 00435 GPIOPFCE4 &= 0xfffe; /* The alternative function of a pin */ 00436 GPIOPFC4 |= 0x0001; /* The alternative function of a pin */ 00437 /* 2nd alternative function/output */ 00438 GPIOP4 &= 0xfffe; /* */ 00439 GPIOPM4 &= 0xfffe; /* p4_0:output */ 00440 GPIOPMC4 |= 0x0001; /* P4_0:double */ 00441 00442 /* Mosule stop 33(MTU2) canceling */ 00443 CPGSTBCR3 &= 0xf7; 00444 00445 /* MTU2_0 (Motor PWM) */ 00446 MTU2TCR_0 = 0x22; /* TCNT Clear(TGRA), P0φ/16 */ 00447 MTU2TIORH_0 = 0x52; /* TGRA L>H, TGRB H>L */ 00448 MTU2TMDR_0 = 0x32; /* TGRC and TGRD = Buff-mode*/ 00449 /* PWM-mode1 */ 00450 MTU2TCNT_0 = 0; /* TCNT0 Set 0 */ 00451 MTU2TGRA_0 = MTU2TGRC_0 = SERVO_PWM_CYCLE; 00452 /* PWM-Cycle(16ms) */ 00453 MTU2TGRB_0 = MTU2TGRD_0 = 0; /* Servo-motor(P4_0) */ 00454 MTU2TSTR |= 0x01; /* TCNT_0 Start */ 00455 } 00456 00457 //Interrupt Timer 00458 //------------------------------------------------------------------// 00459 void intTimer( void ) 00460 { 00461 int x, y; 00462 static int counter = 0; 00463 00464 cnt0++; 00465 cnt1++; 00466 00467 /* Trace by image processing */ 00468 image_SensorAnalog_process( counter ); 00469 00470 ServoControl_process(); 00471 00472 switch( counter++ ) { 00473 case 0: 00474 change_framebuffer_process(); 00475 break; 00476 case 1: 00477 Image_Extraction( ImageData ); 00478 break; 00479 case 2: 00480 Image_Extraction( ImageData ); 00481 break; 00482 case 3: 00483 Image_Extraction( ImageData ); 00484 break; 00485 case 4: 00486 Image_Extraction( ImageData ); 00487 break; 00488 case 5: 00489 Image_Extraction( ImageData ); 00490 break; 00491 case 6: 00492 Image_Extraction( ImageData ); 00493 break; 00494 case 7: 00495 Image_Extraction( ImageData ); 00496 break; 00497 case 8: 00498 Image_Extraction( ImageData ); 00499 break; 00500 case 9: 00501 Image_Compression2( ImageData, 320, ImageComp, 16 ); 00502 break; 00503 case 10: 00504 Image_Compression2( ImageData, 320, ImageComp, 16 ); 00505 break; 00506 case 11: 00507 Binarization_process( ImageComp, ImageBinary, 20*15 ); 00508 break; 00509 case 12: 00510 retDevi_A = Standard_Deviation( TempBinary_A, TempDevi_A, 15 ); 00511 break; 00512 case 13: 00513 retJudgeIM_Max = 0; 00514 for( y = 0; y <= 12; y++ ) { 00515 for( x = 0; x <= 15; x++ ) { 00516 Image_part_Extraction( ImageBinary, 20, x, y, NowImageBinary, 5, 3 ); 00517 retDevi_B = Standard_Deviation( NowImageBinary, NowDevi, 15 ); 00518 retCovari = Covariance( TempDevi_A, NowDevi, 15 ); 00519 retJudgeIM = 0; 00520 retJudgeIM = Judgement_ImageMatching( retCovari, retDevi_A, retDevi_B ); 00521 if( 100 >= retJudgeIM && retJudgeIM > retJudgeIM_Max ) { 00522 X_buff = x; 00523 Y_buff = y; 00524 retJudgeIM_Max = retJudgeIM; 00525 } 00526 } 00527 } 00528 break; 00529 case 33: 00530 counter = 0; 00531 break; 00532 default: 00533 break; 00534 } 00535 00536 led_status_process(); 00537 } 00538 00539 //LED_RGB(on GR-PEACH board) 00540 //------------------------------------------------------------------// 00541 void led_rgb(int led) 00542 { 00543 LED_R = led & 0x1; 00544 LED_G = (led >> 1 ) & 0x1; 00545 LED_B = (led >> 2 ) & 0x1; 00546 } 00547 00548 //user_button_get(on GR-PEACH board) 00549 //------------------------------------------------------------------// 00550 unsigned int user_button_get( void ) 00551 { 00552 return (~user_botton) & 0x1; /* Read ports with switches */ 00553 } 00554 00555 //LED_Status(on GR-PEACH board) Function for only interrupt 00556 //------------------------------------------------------------------// 00557 void led_status_process( void ) 00558 { 00559 static unsigned long led_timer; 00560 int led_set; 00561 int on_time; 00562 int off_time; 00563 00564 /* setting */ 00565 switch( status_set ){ 00566 case RUN: 00567 led_set = LED_GREEN; 00568 on_time = 500; 00569 off_time = 500; 00570 break; 00571 00572 case SENSOR: 00573 led_set = LED_BLUE; 00574 on_time = 50; 00575 off_time = 50; 00576 break; 00577 00578 case MARK: 00579 led_set = LED_RED; 00580 on_time = 250; 00581 off_time = 250; 00582 break; 00583 00584 case STOP: 00585 led_set = LED_RED; 00586 on_time = 1; 00587 off_time = 0; 00588 break; 00589 00590 case ERROR: 00591 led_set = LED_RED; 00592 on_time = 50; 00593 off_time = 50; 00594 break; 00595 00596 default: 00597 led_set = LED_OFF; 00598 on_time = 0; 00599 off_time = 1; 00600 break; 00601 } 00602 00603 /* Display */ 00604 led_timer++; 00605 if( led_timer < on_time ) led_rgb( led_set ); 00606 else if( led_timer < ( on_time + off_time ) ) led_rgb( LED_OFF ); 00607 else led_timer = 0; 00608 } 00609 00610 //LED_Status(on GR-PEACH board) Function for only interrupt 00611 //------------------------------------------------------------------// 00612 void led_status_set( int set ) 00613 { 00614 status_set = set; 00615 } 00616 00617 //led_out(on Motor drive board) 00618 //------------------------------------------------------------------// 00619 void led_out(int led) 00620 { 00621 led = ~led; 00622 LED_3 = led & 0x1; 00623 LED_2 = ( led >> 1 ) & 0x1; 00624 } 00625 00626 //pushsw_get(on Motor drive board) 00627 //------------------------------------------------------------------// 00628 unsigned int pushsw_get( void ) 00629 { 00630 return (~push_sw) & 0x1; /* Read ports with switches */ 00631 } 00632 00633 //motor speed control(PWM) 00634 //Arguments: motor:-100 to 100 00635 //Here, 0 is stop, 100 is forward, -100 is reverse 00636 //------------------------------------------------------------------// 00637 void motor( int accele_l, int accele_r ) 00638 { 00639 accele_l = ( accele_l * MAX_SPEED ) / 100; 00640 accele_r = ( accele_r * MAX_SPEED ) / 100; 00641 00642 /* Left Motor Control */ 00643 if( accele_l >= 0 ) { 00644 /* forward */ 00645 Left_motor_signal = 0; 00646 MTU2TGRC_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * accele_l / 100; 00647 } else { 00648 /* reverse */ 00649 Left_motor_signal = 1; 00650 MTU2TGRC_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * ( -accele_l ) / 100; 00651 } 00652 00653 /* Right Motor Control */ 00654 if( accele_r >= 0 ) { 00655 /* forward */ 00656 Right_motor_signal = 0; 00657 MTU2TGRD_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * accele_r / 100; 00658 } else { 00659 /* reverse */ 00660 Right_motor_signal = 1; 00661 MTU2TGRD_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * ( -accele_r ) / 100; 00662 } 00663 } 00664 00665 //Handle fanction 00666 //------------------------------------------------------------------// 00667 void handle( int angle ) 00668 { 00669 handle_buff = angle; 00670 /* When the servo move from left to right in reverse, replace "-" with "+" */ 00671 MTU2TGRD_0 = SERVO_CENTER - angle * HANDLE_STEP; 00672 } 00673 00674 //Deff fanction 00675 //------------------------------------------------------------------// 00676 int diff( int pwm ) 00677 { 00678 int i, ret; 00679 00680 i = handle_buff; 00681 if( i < 0 ) i = -i; 00682 if( i > 45 ) i = 45; 00683 ret = revolution_difference[i] * pwm / 100; 00684 00685 return ret; 00686 } 00687 00688 //ServoControl_process 00689 //------------------------------------------------------------------// 00690 void ServoControl_process( void ) 00691 { 00692 static int iSensorPattern = 0; 00693 00694 /* -3° ～ +3° */ 00695 iServo = image_sensorAnalog_get() / ANLOG_STEP; 00696 00697 if( !sensor_mode ) { 00698 switch( iSensorPattern ) { 00699 case 0: 00700 switch( image_digital_sensor()&0x0f ) { 00701 case 0x03: 00702 iServo = 14; 00703 break; 00704 case 0x01: 00705 iServo = 20; 00706 iSensorPattern = 11; 00707 break; 00708 case 0x0c: 00709 iServo = -14; 00710 break; 00711 case 0x08: 00712 iServo = -20; 00713 iSensorPattern = 12; 00714 break; 00715 default: 00716 break; 00717 } 00718 break; 00719 00720 case 11: 00721 /* Left side */ 00722 iServo = 23; 00723 if( (image_digital_sensor()&0x0f) == 0x03 ) { 00724 iSensorPattern = 0; 00725 } 00726 break; 00727 00728 case 12: 00729 /* right side */ 00730 iServo = -23; 00731 if( (image_digital_sensor()&0x0f) == 0x0c ) { 00732 iSensorPattern = 0; 00733 } 00734 break; 00735 } 00736 } 00737 } 00738 00739 //Image Data Output( for the Excel ) 00740 //------------------------------------------------------------------// 00741 void ImageData_Serial_Out( unsigned char *Data_Y, int Width ) 00742 { 00743 int Xp, Yp, inc, Height; 00744 00745 Height = (Width / (double)4) * 3; 00746 for( Yp = 0, inc = 0; Yp < Height; Yp++ ) { 00747 for( Xp = 0; Xp < Width; Xp++, inc++ ) { 00748 pc.printf( "%d,", Data_Y[ inc ] ); 00749 } 00750 pc.printf("\n\r"); 00751 } 00752 } 00753 00754 //Image Data Output2( for TeraTerm ) 00755 //------------------------------------------------------------------// 00756 void ImageData_Serial_Out2( unsigned char *Data_Y, int Width ) 00757 { 00758 int Xp, Yp, Height; 00759 00760 Height = (Width / (double)4) * 3; 00761 for( Yp = 0; Yp < Height; Yp++ ) { 00762 for( Xp = 0; Xp < Width; Xp++ ) { 00763 pc.printf( "%d ", Data_Y[Xp + (Yp * Width)] ); 00764 } 00765 pc.printf( "\n\r" ); 00766 } 00767 pc.printf( "\033[%dA" , Height ); 00768 } 00769 00770 //Image Data Output3( for the converter ( csv --> jpg ) ) 00771 //------------------------------------------------------------------// 00772 void ImageData_Serial_Out3( void ) 00773 { 00774 int Xp, Yp, x, y; 00775 00776 /* Camera module test process */ 00777 pc.printf( "//,X-Size,Y-Size" ); 00778 pc.printf( "\n\r" ); 00779 pc.printf( "#SIZE,320,240" ); 00780 pc.printf( "\n\r" ); 00781 pc.printf( "//,X-Point,Y-Point" ); 00782 pc.printf( "\n\r" ); 00783 00784 for( Yp = 0, y = 0; Yp < 240; Yp+=1, y++ ){ 00785 for( Xp = 0, x = 0; Xp < 640; Xp+=4, x+=2 ){ 00786 pc.printf( "#YCbCr," ); 00787 /*Xp*/pc.printf( "%d,", x); 00788 /*Yp*/pc.printf( "%d,", y); 00789 /*Y0*/pc.printf( "%d,", save_buff_addr[(Xp+0)+(640*Yp)]);//6 00790 /*Cb*/pc.printf( "%d,", save_buff_addr[(Xp+1)+(640*Yp)]);//5 00791 /*Cr*/pc.printf( "%d,", save_buff_addr[(Xp+3)+(640*Yp)]);//7 00792 pc.printf( "\n\r" ); 00793 00794 pc.printf( "#YCbCr," ); 00795 /*Xp*/pc.printf( "%d,", x+1); 00796 /*Yp*/pc.printf( "%d,", y); 00797 /*Y1*/pc.printf( "%d,", save_buff_addr[(Xp+2)+(640*Yp)]);//4 00798 /*Cb*/pc.printf( "%d,", save_buff_addr[(Xp+1)+(640*Yp)]);//5 00799 /*Cr*/pc.printf( "%d,", save_buff_addr[(Xp+3)+(640*Yp)]);//7 00800 pc.printf( "\n\r" ); 00801 } 00802 } 00803 } 00804 00805 //Change FrameBuffer Process 00806 //------------------------------------------------------------------// 00807 void change_framebuffer_process( void ) 00808 { 00809 DisplayBase::graphics_error_t error; 00810 DisplayBase Display; 00811 00812 /* Change address buffer */ 00813 if (write_buff_addr == FrameBuffer_Video_A) { 00814 write_buff_addr = FrameBuffer_Video_B; 00815 save_buff_addr = FrameBuffer_Video_A; 00816 } else { 00817 write_buff_addr = FrameBuffer_Video_A; 00818 save_buff_addr = FrameBuffer_Video_B; 00819 } 00820 00821 /* Change write buffer */ 00822 error = Display.Video_Write_Change( 00823 VIDEO_INPUT_CH, 00824 write_buff_addr, 00825 VIDEO_BUFFER_STRIDE); 00826 if (error != DisplayBase::GRAPHICS_OK) { 00827 printf("Line %d, error %d\n", __LINE__, error); 00828 while (1); 00829 } 00830 } 00831 00832 //center_line_corrective_process 00833 //------------------------------------------------------------------// 00834 int center_line_corrective( void ) 00835 { 00836 static int process_No = 0; 00837 static int im_cnt = 0; 00838 static int value = 0; 00839 int iRet = 0; 00840 int image_sensor_value; 00841 00842 image_sensor_value = image_sensorAnalog_get(); 00843 00844 switch( process_No ) { 00845 case 0: 00846 /* center_line_corrective */ 00847 if( (value > image_sensor_value) && (image_sensor_value > -value) ) { 00848 cnt1 = 0; 00849 process_No = 1; 00850 } 00851 if( cnt1 >= 34 ) process_No = 3; 00852 break; 00853 00854 case 1: 00855 /* Retrial after 100ms */ 00856 if( cnt1 >= 68 ) { 00857 if( (value > image_sensor_value) && (image_sensor_value > -value) ) { 00858 cnt1 = 0; 00859 process_No = 2; 00860 } else { 00861 process_No = 3; 00862 } 00863 } 00864 break; 00865 00866 case 2: 00867 iRet = 1; 00868 break; 00869 00870 case 3: 00871 im_offset_x += 2; 00872 if( im_offset_x > ( ((160 - IM_GAP_SET) + (IM_GAP_SET * 2)) * 2 ) ) { 00873 im_offset_x = 160 - IM_GAP_SET; 00874 im_cnt++; 00875 } 00876 if( im_cnt >= 1 ) { 00877 im_cnt = 0; 00878 value += 2; 00879 if( value >= 6 ) { 00880 value = 6; 00881 iRet = -1; 00882 } 00883 } 00884 cnt1 = 0; 00885 process_No = 0; 00886 break; 00887 00888 default: 00889 break; 00890 } 00891 return iRet; 00892 } 00893 00894 //digital_sensor_corrective 00895 //------------------------------------------------------------------// 00896 void digital_sensor_corrective( void ) 00897 { 00898 int Ypix; 00899 int Xpix; 00900 int pix_value; 00901 00902 black = 127; 00903 white = 127; 00904 00905 /* Maximum value of the black and white */ 00906 Ypix = IMAGE_LINE; 00907 for( Xpix = (im_offset_x - 192); Xpix < (im_offset_x + 192); Xpix+=4 ) { 00908 pix_value = save_buff_addr[(Ypix * 640) + (Xpix + 0)];/* Y1 */ 00909 if( black >= pix_value ) black = pix_value; 00910 if( white <= pix_value ) white = pix_value; 00911 00912 pix_value = save_buff_addr[(Ypix * 640) + (Xpix + 2)];/* Y2 */ 00913 if( black >= pix_value ) black = pix_value; 00914 if( white <= pix_value ) white = pix_value; 00915 } 00916 00917 /* threshold value */ 00918 digital_sensor_threshold = ( white - black ) / 2; 00919 00920 /* X3 */ 00921 Ypix = IMAGE_LINE; 00922 for( Xpix = (im_offset_x - 192); Xpix < im_offset_x; Xpix+=4 ) { 00923 pix_value = save_buff_addr[(Ypix * 640) + (Xpix + 0)];/* Y1 */ 00924 pix_value += save_buff_addr[(Ypix * 640) + (Xpix + 2)];/* Y2 */ 00925 pix_value /= 2; 00926 00927 if( pix_value > digital_sensor_threshold ) { 00928 sensor_x[3] = Xpix; 00929 break; 00930 } 00931 } 00932 00933 /* X2 */ 00934 sensor_x[2] = im_offset_x; 00935 00936 /* X1 */ 00937 Ypix = IMAGE_LINE; 00938 for( Xpix = im_offset_x; Xpix < (im_offset_x + 192); Xpix+=4 ) { 00939 pix_value = save_buff_addr[(Ypix * 640) + (Xpix + 0)];/* Y1 */ 00940 pix_value += save_buff_addr[(Ypix * 640) + (Xpix + 2)];/* Y2 */ 00941 pix_value /= 2; 00942 00943 if( pix_value < digital_sensor_threshold ) { 00944 sensor_x[1] = Xpix; 00945 break; 00946 } 00947 } 00948 00949 /* X4 */ 00950 sensor_x[4] = sensor_x[3] - (sensor_x[2] - sensor_x[3]); 00951 00952 /* X0 */ 00953 sensor_x[0] = (sensor_x[1] - sensor_x[2]) + sensor_x[1]; 00954 00955 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] ); 00956 } 00957 00958 //image_SensorAnalog_process 00959 //------------------------------------------------------------------// 00960 void image_SensorAnalog_process( int timer33 ) 00961 { 00962 int Ypix; 00963 int Xpix; 00964 int left_value; 00965 int right_value; 00966 00967 /* Initialization */ 00968 left_value = 0; 00969 right_value = 0; 00970 00971 /* left image value ( 64pix to 160pix ) */ 00972 Ypix = IMAGE_LINE - (timer33);//109 to 75 line 00973 for( Xpix = (im_offset_x - 192); Xpix < im_offset_x; Xpix+=4 ) { 00974 left_value += save_buff_addr[(Ypix * 640) + (Xpix + 0)];/* Y1 */ 00975 left_value += save_buff_addr[(Ypix * 640) + (Xpix + 2)];/* Y2 */ 00976 } 00977 /* right image value ( 161pix to 256pix ) */ 00978 Ypix = IMAGE_LINE - (timer33);//109 to 75 line 00979 for( Xpix = im_offset_x; Xpix < ( im_offset_x + 192 ); Xpix+=4 ) { 00980 right_value += save_buff_addr[(Ypix * 640) + (Xpix + 0)];/* Y1 */ 00981 right_value += save_buff_addr[(Ypix * 640) + (Xpix + 2)];/* Y2 */ 00982 } 00983 00984 Left_sensor = left_value / 10; 00985 Right_sensor = right_value / 10; 00986 } 00987 00988 //image_sensorAnalog_get 00989 //------------------------------------------------------------------// 00990 int image_sensorAnalog_get( void ) 00991 { 00992 int iRet; 00993 00994 iRet = Left_sensor - Right_sensor; 00995 00996 /* Reverse of the positive and negative. */ 00997 iRet *= -1; 00998 00999 if( iRet <= -900 ) iRet = -900;//Left side 01000 if( iRet >= 900 ) iRet = 900;//right side 01001 01002 return iRet; 01003 } 01004 01005 01006 //image_digital_sensor 01007 //------------------------------------------------------------------// 01008 unsigned char image_digital_sensor( void ) 01009 { 01010 int Ypix; 01011 int Xpix; 01012 int data; 01013 unsigned char sensor; 01014 01015 sensor = 0; 01016 01017 /* sensor 2 (center) */ 01018 data = 0; 01019 Ypix = IMAGE_LINE; 01020 for( Xpix = (sensor_x[2] - 8); Xpix < (sensor_x[2] + 8); Xpix+=4 ) { 01021 data += save_buff_addr[(Ypix * 640) + (Xpix + 0)];/* Y1 */ 01022 data += save_buff_addr[(Ypix * 640) + (Xpix + 2)];/* Y2 */ 01023 } 01024 data /= 8; 01025 if( data <= digital_sensor_threshold ) data = 0; 01026 else data = 1; 01027 sensor |= (data << 4) & 0x10; 01028 01029 /* sensor 4 (left side) */ 01030 data = 0; 01031 Ypix = IMAGE_LINE; 01032 for( Xpix = (sensor_x[4] - 16); Xpix < sensor_x[4]; Xpix+=4 ) { 01033 data += save_buff_addr[(Ypix * 640) + (Xpix + 0)];/* Y1 */ 01034 data += save_buff_addr[(Ypix * 640) + (Xpix + 2)];/* Y2 */ 01035 } 01036 data /= 8; 01037 if( data <= digital_sensor_threshold ) data = 0; 01038 else data = 1; 01039 sensor |= (data << 3) & 0x08; 01040 01041 /* sensor 3 (left side) */ 01042 data = 0; 01043 Ypix = IMAGE_LINE; 01044 for( Xpix = (sensor_x[3] - 16); Xpix < sensor_x[3]; Xpix+=4 ) { 01045 data += save_buff_addr[(Ypix * 640) + (Xpix + 0)];/* Y1 */ 01046 data += save_buff_addr[(Ypix * 640) + (Xpix + 2)];/* Y2 */ 01047 } 01048 data /= 8; 01049 if( data <= digital_sensor_threshold ) data = 0; 01050 else data = 1; 01051 sensor |= (data << 2) & 0x04; 01052 01053 /* sensor 1 (right side) */ 01054 data = 0; 01055 Ypix = IMAGE_LINE; 01056 for( Xpix = sensor_x[1]; Xpix < (sensor_x[1] + 16); Xpix+=4 ) { 01057 data += save_buff_addr[(Ypix * 640) + (Xpix + 0)];/* Y1 */ 01058 data += save_buff_addr[(Ypix * 640) + (Xpix + 2)];/* Y2 */ 01059 } 01060 data /= 8; 01061 if( data <= digital_sensor_threshold ) data = 0; 01062 else data = 1; 01063 sensor |= (data << 1) & 0x02; 01064 01065 /* sensor 0 (right side) */ 01066 data = 0; 01067 Ypix = IMAGE_LINE; 01068 for( Xpix = sensor_x[0]; Xpix < (sensor_x[0] + 16); Xpix+=4 ) { 01069 data += save_buff_addr[(Ypix * 640) + (Xpix + 0)];/* Y1 */ 01070 data += save_buff_addr[(Ypix * 640) + (Xpix + 2)];/* Y2 */ 01071 } 01072 data /= 8; 01073 if( data <= digital_sensor_threshold ) data = 0; 01074 else data = 1; 01075 sensor |= (data) & 0x01; 01076 01077 sensor &= 0x1f; 01078 01079 return sensor; 01080 } 01081 01082 //Image Data YCbCr -> Y(320*240pix) 01083 //------------------------------------------------------------------// 01084 void Image_Extraction( unsigned char *Data_Y ) 01085 { 01086 static int Xp, Yp, inc; 01087 static int counter = 0; 01088 #if 0 01089 // Function 01090 for( Yp = 0, inc = 0; Yp < 240; Yp++ ){ 01091 for( Xp = 0; Xp < 640; Xp+=2, inc++ ){ 01092 Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)]; 01093 } 01094 } 01095 #else 01096 // Distributed processing 01097 switch( counter++ ) { 01098 case 0: 01099 for( Yp = 0, inc = 0; Yp < 30; Yp++ ){ 01100 for( Xp = 0; Xp < 640; Xp+=2, inc++ ){ 01101 Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)]; 01102 } 01103 } 01104 break; 01105 case 1: 01106 for( ; Yp < 60; Yp++ ){ 01107 for( Xp = 0; Xp < 640; Xp+=2, inc++ ){ 01108 Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)]; 01109 } 01110 } 01111 break; 01112 case 2: 01113 for( ; Yp < 90; Yp++ ){ 01114 for( Xp = 0; Xp < 640; Xp+=2, inc++ ){ 01115 Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)]; 01116 } 01117 } 01118 break; 01119 case 3: 01120 for( ; Yp < 120; Yp++ ){ 01121 for( Xp = 0; Xp < 640; Xp+=2, inc++ ){ 01122 Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)]; 01123 } 01124 } 01125 break; 01126 case 4: 01127 for( ; Yp < 150; Yp++ ){ 01128 for( Xp = 0; Xp < 640; Xp+=2, inc++ ){ 01129 Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)]; 01130 } 01131 } 01132 break; 01133 case 5: 01134 for( ; Yp < 180; Yp++ ){ 01135 for( Xp = 0; Xp < 640; Xp+=2, inc++ ){ 01136 Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)]; 01137 } 01138 } 01139 break; 01140 case 6: 01141 for( ; Yp < 210; Yp++ ){ 01142 for( Xp = 0; Xp < 640; Xp+=2, inc++ ){ 01143 Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)]; 01144 } 01145 } 01146 break; 01147 case 7: 01148 for( ; Yp < 240; Yp++ ){ 01149 for( Xp = 0; Xp < 640; Xp+=2, inc++ ){ 01150 Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)]; 01151 } 01152 } 01153 counter = 0; 01154 break; 01155 default: 01156 break; 01157 } 01158 #endif 01159 01160 } 01161 01162 //Image_Compression Y ( Thinning processing ) 01163 //------------------------------------------------------------------// 01164 void Image_Compression( unsigned char *Data_Y, int Data_W , unsigned char *Comp_Y, int Comp_W ) 01165 { 01166 int Data_H, Comp_H, Step_W, Step_H; 01167 int Xp, Yp, inc; 01168 01169 Data_H = (Data_W / (double)4) * 3; 01170 Comp_H = (Comp_W / (double)4) * 3; 01171 Step_W = Data_W / Comp_W; 01172 Step_H = Data_H / Comp_H; 01173 01174 for( Yp = 0, inc = 0; Yp < Comp_H; Yp++ ) { 01175 for( Xp = 0; Xp < Comp_W; Xp++, inc++ ) { 01176 Comp_Y[inc] = Data_Y[(Yp * (Data_W * Step_H)) + (Xp * Step_W)]; 01177 } 01178 } 01179 } 01180 01181 //Image_Compression2 Y ( Averaging processing ) 01182 //------------------------------------------------------------------// 01183 void Image_Compression2( unsigned char *Data_Y, int Data_W , unsigned char *Comp_Y, int Comp_M ) 01184 { 01185 int Data_H, Pixel_T, Pixel_D; 01186 int x, y; 01187 static int Xp, Yp, inc; 01188 static int counter = 0; 01189 01190 Data_H = (Data_W / (double)4) * 3; 01191 Pixel_D = Comp_M * Comp_M; 01192 #if 0 01193 // Function 01194 for( Yp = 0, inc = 0; Yp < Data_H; Yp+=Comp_M ){ 01195 for( Xp = 0; Xp < Data_W; Xp+=Comp_M, inc++ ){ 01196 Pixel_T = 0; 01197 for( y = 0; y < Comp_M; y++ ){ 01198 for( x = 0; x < Comp_M; x++ ){ 01199 Pixel_T += Data_Y[( Xp + x ) + (( Yp + y ) * Data_W )]; 01200 } 01201 } 01202 Comp_Y[inc] = Pixel_T / Pixel_D; 01203 } 01204 } 01205 #else 01206 // Distributed processing 01207 switch( counter++ ) { 01208 case 0: 01209 for( Yp = 0, inc = 0; Yp < (Data_H / 2); Yp+=Comp_M ){ 01210 for( Xp = 0; Xp < Data_W; Xp+=Comp_M, inc++ ){ 01211 Pixel_T = 0; 01212 for( y = 0; y < Comp_M; y++ ){ 01213 for( x = 0; x < Comp_M; x++ ){ 01214 Pixel_T += Data_Y[( Xp + x ) + (( Yp + y ) * Data_W )]; 01215 } 01216 } 01217 Comp_Y[inc] = Pixel_T / Pixel_D; 01218 } 01219 } 01220 break; 01221 case 1: 01222 for( ; Yp < Data_H; Yp+=Comp_M ){ 01223 for( Xp = 0; Xp < Data_W; Xp+=Comp_M, inc++ ){ 01224 Pixel_T = 0; 01225 for( y = 0; y < Comp_M; y++ ){ 01226 for( x = 0; x < Comp_M; x++ ){ 01227 Pixel_T += Data_Y[( Xp + x ) + (( Yp + y ) * Data_W )]; 01228 } 01229 } 01230 Comp_Y[inc] = Pixel_T / Pixel_D; 01231 } 01232 } 01233 counter = 0; 01234 break; 01235 default: 01236 break; 01237 } 01238 #endif 01239 } 01240 01241 // Binarization_process 01242 //------------------------------------------------------------------// 01243 void Binarization_process( unsigned char *Comp_Y, unsigned char *Binary, long items ) 01244 { 01245 int i, threshold; 01246 01247 threshold = 150; 01248 for( i = 0; i < items; i++ ) { 01249 if( Comp_Y[i] >= threshold ) Binary[i] = 1; 01250 else Binary[i] = 0; 01251 } 01252 } 01253 01254 // Extract_Image 01255 //------------------------------------------------------------------// 01256 void Image_part_Extraction( unsigned char *Binary, int Width, int Xpix, int Ypix, unsigned char *Data_B, int x_size, int y_size ) 01257 { 01258 int x, y; 01259 for( y = 0; y < y_size; y++ ) { 01260 for( x = 0; x < x_size; x++ ) { 01261 Data_B[ x + ( y * x_size ) ] = Binary[ (Xpix + x) + ( (Ypix + y) * Width ) ]; 01262 } 01263 } 01264 } 01265 01266 // Standard deviation 01267 //------------------------------------------------------------------// 01268 double Standard_Deviation( unsigned char *data, double *Devi, int items ) 01269 { 01270 int i; 01271 double iRet_A, iRet_C, iRet_D; 01272 01273 /* A 合計値 平均化 */ 01274 iRet_A = 0; 01275 for( i = 0; i < items; i++ ) { 01276 iRet_A += data[i]; 01277 } 01278 iRet_A /= items; 01279 01280 /* B 偏差値 */ 01281 for( i = 0; i < items; i++ ) { 01282 Devi[i] = data[i] - iRet_A; 01283 } 01284 01285 /* C 分散 */ 01286 iRet_C = 0; 01287 for( i = 0; i < items; i++ ) { 01288 iRet_C += ( Devi[i] * Devi[i] ); 01289 } 01290 iRet_C /= items; 01291 01292 /* D 標準偏差 */ 01293 iRet_D = sqrt( iRet_C ); 01294 01295 return iRet_D; 01296 } 01297 01298 // Covariance 01299 //------------------------------------------------------------------// 01300 double Covariance( double *Devi_A, double *Devi_B, int items ) 01301 { 01302 int i; 01303 double iRet, iRet_buff; 01304 01305 iRet = 0; 01306 for( i = 0; i < items; i++ ) { 01307 iRet_buff = Devi_A[i] * Devi_B[i]; 01308 iRet += iRet_buff; 01309 } 01310 iRet /= items; 01311 01312 return iRet; 01313 } 01314 01315 // Judgement_ImageMatching 01316 //------------------------------------------------------------------// 01317 int Judgement_ImageMatching( double covari, double SDevi_A, double SDevi_B ) 01318 { 01319 int iRet; 01320 01321 iRet = ( covari * 100 ) / ( SDevi_A * SDevi_B ); 01322 01323 return iRet; 01324 } 01325 01326 //******************************************************************// 01327 // @brief Interrupt callback function 01328 // @param[in] int_type : VDC5 interrupt type 01329 // @retval None 01330 //*******************************************************************/ 01331 static void IntCallbackFunc_Vfield(DisplayBase::int_type_t int_type) 01332 { 01333 if (vfield_count > 0) { 01334 vfield_count--; 01335 } 01336 } 01337 01338 //******************************************************************// 01339 // @brief Wait for the specified number of times Vsync occurs 01340 // @param[in] wait_count : Wait count 01341 // @retval None 01342 //*******************************************************************/ 01343 static void WaitVfield(const int32_t wait_count) 01344 { 01345 vfield_count = wait_count; 01346 while (vfield_count > 0) { 01347 /* Do nothing */ 01348 } 01349 } 01350 01351 //******************************************************************// 01352 // @brief Interrupt callback function for Vsync interruption 01353 // @param[in] int_type : VDC5 interrupt type 01354 // @retval None 01355 //*******************************************************************/ 01356 static void IntCallbackFunc_Vsync(DisplayBase::int_type_t int_type) 01357 { 01358 if (vsync_count > 0) { 01359 vsync_count--; 01360 } 01361 } 01362 01363 //******************************************************************// 01364 // @brief Wait for the specified number of times Vsync occurs 01365 // @param[in] wait_count : Wait count 01366 // @retval None 01367 //*******************************************************************/ 01368 static void WaitVsync(const int32_t wait_count) 01369 { 01370 vsync_count = wait_count; 01371 while (vsync_count > 0) { 01372 /* Do nothing */ 01373 } 01374 } 01375 01376 //------------------------------------------------------------------// 01377 // End of file 01378 //------------------------------------------------------------------//
Generated on Tue Jul 19 2022 04:07:48 by
1.7.2