Micon Car Rally
/
TraceMark_Program_60fps_Ver2
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: GR-PEACH_video mbed
Fork of
TraceMark_Program_60fps
by 
Micon Car Rally
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 //------------------------------------------------------------------// 00002 //Supported MCU: RZ/A1H 00003 //File Contents: Trace Program by Image Processing 00004 // (GR-PEACH version on the Micon Car) 00005 //Version number: Ver.2.00 00006 //Date: 2017.03.17 00007 //Copyright: Renesas Electronics Corporation 00008 // Hitachi Document Solutions Co., Ltd. 00009 //------------------------------------------------------------------// 00010 00011 //This program supports the following boards: 00012 //* GR-PEACH(E version) 00013 //* Motor drive board Ver.5 00014 //* Camera module (SC-310) 00015 00016 //Include 00017 //------------------------------------------------------------------// 00018 #include "mbed.h" 00019 #include "math.h" 00020 #include "iodefine.h" 00021 #include "DisplayBace.h" 00022 00023 //Define 00024 //------------------------------------------------------------------// 00025 //Motor PWM cycle 00026 #define MOTOR_PWM_CYCLE 33332 /* Motor PWM period */ 00027 /* 1ms P0φ/1 = 0.03us */ 00028 //Motor speed 00029 #define MAX_SPEED 40 /* motor() set: 0 to 100 */ 00030 00031 //Servo PWM cycle 00032 #define SERVO_PWM_CYCLE 33332 /* SERVO PWM period */ 00033 /* 16ms P0φ/16 = 0.48us */ 00034 #define SERVO_CENTER 3124 /* 1.5ms / 0.48us - 1 = 3124*/ 00035 #define HANDLE_STEP 18 /* 1 degree value */ 00036 00037 //LED Color on GR-PEACH 00038 #define LED_OFF 0x00 00039 #define LED_RED 0x01 00040 #define LED_GREEN 0x02 00041 #define LED_YELLOW 0x03 00042 #define LED_BLUE 0x04 00043 #define LED_PURPLE 0x05 00044 #define LED_SKYBLUE 0x06 00045 #define LED_WHITE 0x07 00046 00047 //Status 00048 #define ERROR 0x00 00049 #define STOP 0x01 00050 #define RUN 0x02 00051 #define DEBUG 0x03 00052 #define MOTOR_START 0x04 00053 #define MOTOR_STOP 0x05 00054 #define MARK_T 0x06 00055 #define MARK_C 0x07 00056 #define MARK_R 0x08 00057 #define MARK_L 0x09 00058 00059 //Define(NTSC-Video) 00060 //------------------------------------------------------------------// 00061 #define VIDEO_INPUT_CH (DisplayBase::VIDEO_INPUT_CHANNEL_0) 00062 #define VIDEO_INT_TYPE (DisplayBase::INT_TYPE_S0_VFIELD) 00063 #define DATA_SIZE_PER_PIC (2u) 00064 00065 /*! Frame buffer stride: Frame buffer stride should be set to a multiple of 32 or 128 00066 in accordance with the frame buffer burst transfer mode. */ 00067 #define PIXEL_HW (320u) /* QVGA */ 00068 #define PIXEL_VW (240u) /* QVGA */ 00069 #define VIDEO_BUFFER_STRIDE (((PIXEL_HW * DATA_SIZE_PER_PIC) + 31u) & ~31u) 00070 #define VIDEO_BUFFER_HEIGHT (PIXEL_VW) 00071 00072 //Constructor 00073 //------------------------------------------------------------------// 00074 Ticker interrput; 00075 Serial pc(USBTX, USBRX); 00076 DigitalOut LED_R(P6_13); /* LED1 on the GR-PEACH board */ 00077 DigitalOut LED_G(P6_14); /* LED2 on the GR-PEACH board */ 00078 DigitalOut LED_B(P6_15); /* LED3 on the GR-PEACH board */ 00079 DigitalOut USER_LED(P6_12); /* USER_LED 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(NTSC-video) 00089 //------------------------------------------------------------------// 00090 static void IntCallbackFunc_Vfield(DisplayBase::int_type_t int_type); 00091 static void WaitVfield(const int32_t wait_count); 00092 static void IntCallbackFunc_Vsync(DisplayBase::int_type_t int_type); 00093 static void WaitVsync(const int32_t wait_count); 00094 00095 //Prototype 00096 //------------------------------------------------------------------// 00097 //Peripheral functions 00098 void init_MTU2_PWM_Motor( void ); /* Initialize PWM functions */ 00099 void init_MTU2_PWM_Servo( void ); /* Initialize PWM functions */ 00100 void intTimer( void ); /* Interrupt fanction */ 00101 00102 //GR-peach board 00103 void led_rgb(int led); 00104 void led_m_user( int led ); 00105 unsigned int user_button_get( void ); 00106 void led_m_set( int set ); 00107 void led_m_process( void ); /* Function for only interrupt */ 00108 00109 //Motor drive board 00110 void led_out(int led); 00111 unsigned int pushsw_get( void ); 00112 void motor( int accele_l, int accele_r ); 00113 void motor2( int accele_l, int accele_r ); 00114 void handle( int angle ); 00115 int diff( int pwm ); 00116 00117 //Prototype(Image process) 00118 //------------------------------------------------------------------// 00119 void Image_Extraction( unsigned char *buff_addr, unsigned char *Data_Y, int frame ); 00120 void Image_Reduction( unsigned char *Data_Y, int Data_W , unsigned char *Comp_Y, int Comp_M ); 00121 void Binarization_process( unsigned char *Comp_Y, unsigned char *Binary, long items, int threshold ); 00122 00123 //Prototype(Digital sensor process) 00124 //------------------------------------------------------------------// 00125 int CenterLine_Corrective( unsigned char *Binary ); 00126 void digital_sensor_process( unsigned char *Binary ); /* Function for only interrupt */ 00127 unsigned char digital_sensor( void ); 00128 00129 //Prototype(Mark detection process) 00130 //------------------------------------------------------------------// 00131 void Image_part_Extraction( unsigned char *Binary, int Width, int Xpix, int Ypix, unsigned char *Data_B, int x_size, int y_size ); 00132 double Standard_Deviation( unsigned char *data, double *Devi, int items ); 00133 double Covariance( double *Devi_A, double *Devi_B, int items ); 00134 int Judgement_ImageMatching( double covari, double SDevi_A, double SDevi_B ); 00135 00136 void MarkDetect_process_T( void ); 00137 void MarkDetect_process_C( void ); 00138 void MarkDetect_process_R( void ); 00139 void MarkDetect_process_L( void ); 00140 00141 int MarkCheck_Triangle( int percentage ); 00142 int MarkCheck_Crossline( int percentage ); 00143 int MarkCheck_Rightline( int percentage ); 00144 int MarkCheck_Leftline( int percentage ); 00145 00146 //Prototype(Display Debug) 00147 //------------------------------------------------------------------// 00148 void ImageData_Serial_Out( unsigned char *Data_Y, int Width ); 00149 void ImageData_Serial_Out2( unsigned char *Data_Y, int Width ); 00150 00151 //Globle variable (NTSC-video) 00152 //------------------------------------------------------------------// 00153 static uint8_t FrameBuffer_Video_A[VIDEO_BUFFER_STRIDE * VIDEO_BUFFER_HEIGHT]__attribute((section("NC_BSS"),aligned(16))); //16 bytes aligned!; 00154 uint8_t * write_buff_addr = FrameBuffer_Video_A; 00155 static volatile int32_t vsync_count; 00156 static volatile int32_t vfield_count; 00157 static volatile int32_t vfield_count2 = 1; 00158 static volatile int32_t vfield_count2_buff; 00159 00160 //Globle variable for Image process 00161 //------------------------------------------------------------------// 00162 unsigned char ImageData_A[160*120]; 00163 unsigned char ImageComp_A[20*15]; 00164 unsigned char ImageBinary[20*15]; 00165 00166 //Globle variable for Digital sensor process 00167 //------------------------------------------------------------------// 00168 volatile int Sensor_X[8][6]; 00169 volatile unsigned char sensor_value; 00170 00171 //Globle variable for Mark detection process 00172 //------------------------------------------------------------------// 00173 double TempDevi_Triangle[15]; 00174 unsigned char TempBinary_Triangle[15] = {0,1,1,1,0, 00175 0,0,1,0,0, 00176 0,0,0,0,0}; 00177 double TempDevi_Crossline[28]; 00178 unsigned char TempBinary_Crossline[28] = {0,0,0,0,0,1,1,1,1,0,0,0,0,0, 00179 1,1,1,1,1,1,1,1,1,1,1,1,1,1}; 00180 double TempDevi_Rightline[28]; 00181 unsigned char TempBinary_Rightline[28] = {0,0,0,0,0,1,1,1,1,0,0,0,0,0, 00182 0,0,0,0,0,0,1,1,1,1,1,1,1,1}; 00183 double TempDevi_Leftline[28]; 00184 unsigned char TempBinary_Leftline[28] = {0,0,0,0,0,1,1,1,1,0,0,0,0,0, 00185 1,1,1,1,1,1,1,1,0,0,0,0,0,0}; 00186 double NowDevi[30]; 00187 unsigned char NowImageBinary[30]; 00188 00189 volatile double retDevi_Triangle; 00190 volatile double retDevi_Crossline; 00191 volatile double retDevi_Rightline; 00192 volatile double retDevi_Leftline; 00193 00194 volatile double retDevi; 00195 volatile double retCovari; 00196 volatile int retJudgeIM; 00197 volatile int retJudgeIM_Max[4]; 00198 00199 int Xt, Yt; 00200 int Xc, Yc; 00201 int Xr, Yr; 00202 int Xl, Yl; 00203 00204 //Globle variable for led fanction 00205 //------------------------------------------------------------------// 00206 volatile int led_set; /* Status */ 00207 00208 // LED, OnTime, OffTime, 00209 volatile int led_data[10][3]= {LED_RED, 50, 50, /* ERROR */ 00210 LED_RED, 500, 0, /* STOP */ 00211 LED_GREEN, 500, 500, /* RUN */ 00212 LED_BLUE, 50, 50, /* DEBUG */ 00213 LED_GREEN, 1, 0, /* MOTOR_START */ 00214 LED_RED, 1, 0, /* MOTOR_STOP */ 00215 LED_WHITE, 500, 500, /* MARK_T */ 00216 LED_YELLOW, 500, 500, /* MARK_C */ 00217 LED_PURPLE, 500, 500, /* MARK_R */ 00218 LED_SKYBLUE,500, 500}; /* MARK_L */ 00219 00220 //Globle variable for Trace program 00221 //------------------------------------------------------------------// 00222 volatile unsigned long cnt0; /* Used by timer function */ 00223 volatile unsigned long cnt1; /* Used within main */ 00224 volatile int pattern; /* Pattern numbers */ 00225 volatile int handle_buff; 00226 00227 volatile unsigned long main_cnt = 0; /* Used by timer function */ 00228 00229 const int revolution_difference[] = { 00230 100, 98, 97, 95, 93, 00231 92, 90, 88, 87, 85, 00232 84, 82, 81, 79, 78, 00233 76, 75, 73, 72, 71, 00234 69, 68, 66, 65, 64, 00235 62, 61, 59, 58, 57, 00236 55, 54, 52, 51, 50, 00237 48, 47, 45, 44, 42, 00238 41, 39, 38, 36, 35, 00239 33 }; 00240 00241 //******************************************************************// 00242 // Main function 00243 //*******************************************************************/ 00244 int main( void ) 00245 { 00246 /* NTSC-Video */ 00247 DisplayBase::graphics_error_t error; 00248 00249 /* Create DisplayBase object */ 00250 DisplayBase Display; 00251 00252 /* Graphics initialization process */ 00253 error = Display.Graphics_init(NULL); 00254 if (error != DisplayBase::GRAPHICS_OK) { 00255 printf("Line %d, error %d\n", __LINE__, error); 00256 while (1); 00257 } 00258 00259 error = Display.Graphics_Video_init( DisplayBase::INPUT_SEL_VDEC, NULL); 00260 if( error != DisplayBase::GRAPHICS_OK ) { 00261 while(1); 00262 } 00263 00264 /* Interrupt callback function setting (Vsync signal input to scaler 0) */ 00265 error = Display.Graphics_Irq_Handler_Set(DisplayBase::INT_TYPE_S0_VI_VSYNC, 0, IntCallbackFunc_Vsync); 00266 if (error != DisplayBase::GRAPHICS_OK) { 00267 printf("Line %d, error %d\n", __LINE__, error); 00268 while (1); 00269 } 00270 00271 /* Video capture setting (progressive form fixed) */ 00272 error = Display.Video_Write_Setting( 00273 VIDEO_INPUT_CH, 00274 DisplayBase::COL_SYS_NTSC_358, 00275 write_buff_addr, 00276 VIDEO_BUFFER_STRIDE, 00277 DisplayBase::VIDEO_FORMAT_YCBCR422, 00278 DisplayBase::WR_RD_WRSWA_32_16BIT, 00279 PIXEL_VW, 00280 PIXEL_HW 00281 ); 00282 if (error != DisplayBase::GRAPHICS_OK) { 00283 printf("Line %d, error %d\n", __LINE__, error); 00284 while (1); 00285 } 00286 00287 /* Interrupt callback function setting (Field end signal for recording function in scaler 0) */ 00288 error = Display.Graphics_Irq_Handler_Set(VIDEO_INT_TYPE, 0, IntCallbackFunc_Vfield); 00289 if (error != DisplayBase::GRAPHICS_OK) { 00290 printf("Line %d, error %d\n", __LINE__, error); 00291 while (1); 00292 } 00293 00294 /* Video write process start */ 00295 error = Display.Video_Start (VIDEO_INPUT_CH); 00296 if (error != DisplayBase::GRAPHICS_OK) { 00297 printf("Line %d, error %d\n", __LINE__, error); 00298 while (1); 00299 } 00300 00301 /* Video write process stop */ 00302 error = Display.Video_Stop (VIDEO_INPUT_CH); 00303 if (error != DisplayBase::GRAPHICS_OK) { 00304 printf("Line %d, error %d\n", __LINE__, error); 00305 while (1); 00306 } 00307 00308 /* Video write process start */ 00309 error = Display.Video_Start (VIDEO_INPUT_CH); 00310 if (error != DisplayBase::GRAPHICS_OK) { 00311 printf("Line %d, error %d\n", __LINE__, error); 00312 while (1); 00313 } 00314 00315 /* Wait vsync to update resister */ 00316 WaitVsync(1); 00317 00318 /* Wait 2 Vfield(Top or bottom field) */ 00319 WaitVfield(2); 00320 00321 /* Initialize MCU functions */ 00322 init_MTU2_PWM_Motor(); 00323 init_MTU2_PWM_Servo(); 00324 interrput.attach(&intTimer, 0.001); 00325 pc.baud(230400); 00326 00327 /* Initialize Micon Car state */ 00328 led_out( 0x0 ); 00329 handle( 0 ); 00330 motor( 0, 0 ); 00331 00332 /* wait to stabilize NTSC signal (about 170ms) */ 00333 wait(0.2); 00334 00335 /* Initialize Digital sensor */ 00336 CenterLine_Corrective( ImageBinary ); 00337 00338 /* Initialize Mark detection */ 00339 retDevi_Triangle = Standard_Deviation( TempBinary_Triangle, TempDevi_Triangle, 15 ); 00340 retDevi_Crossline = Standard_Deviation( TempBinary_Crossline, TempDevi_Crossline, 28 ); 00341 retDevi_Rightline = Standard_Deviation( TempBinary_Rightline, TempDevi_Rightline, 28 ); 00342 retDevi_Leftline = Standard_Deviation( TempBinary_Leftline, TempDevi_Leftline, 28 ); 00343 00344 if( user_button_get() ) { 00345 wait(0.1); 00346 led_m_set( DEBUG ); 00347 while( user_button_get() ); 00348 wait(0.5); 00349 pc.printf( "Please push the SW ( on the Motor drive board )\n\r" ); 00350 pc.printf( "\n\r" ); 00351 while( !pushsw_get() ); 00352 while( 1 ){ 00353 ImageData_Serial_Out2( ImageBinary, 20 ); 00354 } 00355 } 00356 led_m_set( RUN ); 00357 00358 while( 1 ) { 00359 00360 switch( pattern ) { 00361 /***************************************************************** 00362 About patern 00363 0:wait for switch input 00364 1:check if start bar is open 00365 11:normal trace 00366 12:Left side 00367 13:right side 00368 *****************************************************************/ 00369 case 0: 00370 /* wait for switch input */ 00371 if( pushsw_get() ) { 00372 led_out( 0x0 ); 00373 led_m_set( RUN ); 00374 pattern = 11; 00375 cnt1 = 0; 00376 break; 00377 } 00378 if( cnt1 < 100 ) { 00379 led_out( 0x1 ); 00380 } else if( cnt1 < 200 ) { 00381 led_out( 0x2 ); 00382 } else { 00383 cnt1 = 0; 00384 } 00385 break; 00386 00387 case 11: 00388 /* normal trace */ 00389 if( MarkCheck_Triangle( 90 ) ) { 00390 led_m_set( MARK_T ); 00391 pattern = 91; 00392 break; 00393 } 00394 if( MarkCheck_Crossline( 90 ) ) { /* Cross line check */ 00395 led_m_set( MARK_C ); 00396 pattern = 21; 00397 break; 00398 } 00399 if( MarkCheck_Rightline( 90 ) ) { /* Right half line detection check */ 00400 led_m_set( MARK_R ); 00401 pattern = 51; 00402 break; 00403 } 00404 if( MarkCheck_Leftline( 90 ) ) { /* Left half line detection check */ 00405 led_m_set( MARK_L ); 00406 pattern = 61; 00407 break; 00408 } 00409 switch( (digital_sensor()&0x0f) ) { 00410 case 0x00: 00411 handle( 0 ); 00412 motor( 100, 100 ); 00413 break; 00414 case 0x02: 00415 handle( 3 ); 00416 motor( 100, diff(100) ); 00417 break; 00418 case 0x03: 00419 handle( 12 ); 00420 motor( 100, diff(100) ); 00421 break; 00422 case 0x01: 00423 handle( 20 ); 00424 motor( 100, diff(100) ); 00425 pattern = 12; 00426 break; 00427 case 0x04: 00428 handle( -3 ); 00429 motor( diff(100), 100 ); 00430 break; 00431 case 0x0c: 00432 handle( -12 ); 00433 motor( diff(100), 100 ); 00434 break; 00435 case 0x08: 00436 handle( -20 ); 00437 motor( diff(100), 100 ); 00438 pattern = 13; 00439 break; 00440 default: 00441 break; 00442 } 00443 break; 00444 00445 case 12: 00446 /* Left side */ 00447 if( (digital_sensor()&0x02) == 0x02 ) { 00448 pattern = 11; 00449 break; 00450 } 00451 switch( (digital_sensor()&0x0f) ) { 00452 case 0x01: 00453 handle( 20 ); 00454 motor( 100, diff(100) ); 00455 break; 00456 case 0x00: 00457 case 0x08: 00458 case 0x0c: 00459 handle( 22 ); 00460 motor( 100, diff(100) ); 00461 break; 00462 default: 00463 break; 00464 } 00465 break; 00466 00467 case 13: 00468 /* right side */ 00469 if( (digital_sensor()&0x04) == 0x04 ) { 00470 pattern = 11; 00471 } 00472 switch( (digital_sensor()&0x0f) ) { 00473 case 0x08: 00474 handle( -20 ); 00475 motor( diff(100), 100 ); 00476 break; 00477 case 0x00: 00478 case 0x01: 00479 case 0x03: 00480 handle( -22 ); 00481 motor( diff(100), 100 ); 00482 break; 00483 default: 00484 break; 00485 } 00486 break; 00487 00488 case 21: 00489 /* Processing at 1st cross line */ 00490 led_out( 0x3 ); 00491 handle( 0 ); 00492 motor2( 0 ,0 ); 00493 pattern = 22; 00494 cnt1 = 0; 00495 break; 00496 00497 case 22: 00498 /* Read but ignore 2nd line */ 00499 if( cnt1 > 100 ) { 00500 pattern = 23; 00501 cnt1 = 0; 00502 } 00503 break; 00504 00505 case 23: 00506 /* Trace, crank detection after cross line */ 00507 if( (digital_sensor()&0x0c) == 0x0c) { 00508 /* Left crank determined -> to left crank clearing processing */ 00509 led_out( 0x1 ); 00510 pattern = 31; 00511 cnt1 = 0; 00512 break; 00513 } 00514 if( (digital_sensor()&0x03) == 0x03 ) { 00515 /* Right crank determined -> to right crank clearing processing */ 00516 led_out( 0x2 ); 00517 pattern = 41; 00518 cnt1 = 0; 00519 break; 00520 } 00521 switch( (digital_sensor()&0x0f) ) { 00522 case 0x00: 00523 /* Center -> straight */ 00524 handle( 0 ); 00525 motor2( 27 ,27 ); 00526 break; 00527 case 0x02: 00528 /* Left of center -> turn to right */ 00529 handle( 4 ); 00530 motor2( 27 ,diff(27) ); 00531 break; 00532 case 0x04: 00533 /* Right of center -> turn to left */ 00534 handle( -4 ); 00535 motor2( diff(27) ,27 ); 00536 break; 00537 default: 00538 break; 00539 } 00540 break; 00541 00542 case 31: 00543 if( (digital_sensor()&0x0f) == 0x00 ) { 00544 handle( -40 ); 00545 motor2( diff(25) ,45 ); 00546 pattern = 32; 00547 cnt1 = 0; 00548 } 00549 break; 00550 00551 case 32: 00552 /* Left crank clearing processing - wait until stable */ 00553 if( cnt1 > 300 ) { 00554 pattern = 33; 00555 cnt1 = 0; 00556 } 00557 break; 00558 00559 case 33: 00560 /* Left crank clearing processing - check end of turn */ 00561 if( (digital_sensor()&0x04) == 0x04 ) { 00562 led_out( 0x0 ); 00563 led_m_set( RUN ); 00564 pattern = 11; 00565 cnt1 = 0; 00566 } 00567 break; 00568 00569 case 41: 00570 if( (digital_sensor()&0x0f) == 0x00 ) { 00571 handle( 41 ); 00572 motor2( 50 ,diff(25) ); 00573 pattern = 42; 00574 cnt1 = 0; 00575 } 00576 break; 00577 00578 case 42: 00579 /* Right crank clearing processing - wait until stable */ 00580 if( cnt1 > 300 ) { 00581 pattern = 43; 00582 cnt1 = 0; 00583 } 00584 break; 00585 00586 case 43: 00587 /* Right crank clearing processing - check end of turn */ 00588 if( (digital_sensor()&0x02) == 0x02 ) { 00589 led_out( 0x0 ); 00590 led_m_set( RUN ); 00591 pattern = 11; 00592 cnt1 = 0; 00593 } 00594 break; 00595 00596 case 51: 00597 /* Processing at 1st right half line detection */ 00598 led_out( 0x2 ); 00599 handle( 0 ); 00600 motor2( 0 ,0 ); 00601 pattern = 52; 00602 cnt1 = 0; 00603 break; 00604 00605 case 52: 00606 /* Read but ignore 2nd time */ 00607 if( cnt1 > 100 ) { 00608 pattern = 53; 00609 cnt1 = 0; 00610 break; 00611 } 00612 if( MarkCheck_Crossline( 90 ) ) { /* Cross line check */ 00613 led_m_set( MARK_C ); 00614 pattern = 21; 00615 break; 00616 } 00617 break; 00618 00619 case 53: 00620 /* Trace, lane change after right half line detection */ 00621 if( (digital_sensor()&0x1f) == 0x00 ) { 00622 handle( 22 ); 00623 motor2( 35 ,diff(25) ); 00624 pattern = 54; 00625 cnt1 = 0; 00626 break; 00627 } 00628 switch( (digital_sensor()&0x0f) ) { 00629 case 0x00: 00630 /* Center -> straight */ 00631 handle( 0 ); 00632 motor2( 30 ,30 ); 00633 break; 00634 case 0x02: 00635 /* Left of center -> turn to right */ 00636 handle( 4 ); 00637 motor2( 30 ,diff(30) ); 00638 break; 00639 case 0x04: 00640 /* Right of center -> turn to left */ 00641 handle( -4 ); 00642 motor2( diff(30) ,30 ); 00643 break; 00644 default: 00645 break; 00646 } 00647 break; 00648 00649 case 54: 00650 if( cnt1 > 500 ){ 00651 pattern = 55; 00652 cnt1 = 0; 00653 } 00654 break; 00655 00656 case 55: 00657 /* Right lane change end check */ 00658 if( (digital_sensor()&0x04) == 0x04 ) { 00659 led_out( 0x0 ); 00660 led_m_set( RUN ); 00661 pattern = 11; 00662 cnt1 = 0; 00663 } 00664 break; 00665 00666 case 61: 00667 /* Processing at 1st left half line detection */ 00668 led_out( 0x1 ); 00669 handle( 0 ); 00670 motor2( 0 ,0 ); 00671 pattern = 62; 00672 cnt1 = 0; 00673 break; 00674 00675 case 62: 00676 /* Read but ignore 2nd time */ 00677 if( cnt1 > 100 ) { 00678 pattern = 63; 00679 cnt1 = 0; 00680 break; 00681 } 00682 if( MarkCheck_Crossline( 90 ) ) { /* Cross line check */ 00683 led_m_set( MARK_C ); 00684 pattern = 21; 00685 break; 00686 } 00687 break; 00688 00689 case 63: 00690 /* Trace, lane change after left half line detection */ 00691 if( (digital_sensor()&0x1f) == 0x00 ) { 00692 handle( -22 ); 00693 motor2( diff(25) ,35 ); 00694 pattern = 64; 00695 cnt1 = 0; 00696 break; 00697 } 00698 switch( (digital_sensor()&0x0f) ) { 00699 case 0x00: 00700 /* Center -> straight */ 00701 handle( 0 ); 00702 motor2( 35 ,35 ); 00703 break; 00704 case 0x02: 00705 /* Left of center -> turn to right */ 00706 handle( 4 ); 00707 motor2( 35 ,diff(35) ); 00708 break; 00709 case 0x04: 00710 /* Right of center -> turn to left */ 00711 handle( -4 ); 00712 motor2( diff(35) ,35 ); 00713 break; 00714 default: 00715 break; 00716 } 00717 break; 00718 00719 case 64: 00720 if( cnt1 > 500 ){ 00721 pattern = 65; 00722 cnt1 = 0; 00723 } 00724 break; 00725 00726 case 65: 00727 /* Left lane change end check */ 00728 if( (digital_sensor()&0x02) == 0x02 ) { 00729 led_out( 0x0 ); 00730 led_m_set( RUN ); 00731 pattern = 11; 00732 cnt1 = 0; 00733 } 00734 break; 00735 00736 case 91: 00737 handle( 0 ); 00738 motor( 0, 0 ); 00739 break; 00740 00741 default: 00742 break; 00743 } 00744 } 00745 } 00746 00747 //******************************************************************// 00748 // Initialize functions 00749 //*******************************************************************/ 00750 //Initialize MTU2 PWM functions 00751 //------------------------------------------------------------------// 00752 //MTU2_3, MTU2_4 00753 //Reset-Synchronized PWM mode 00754 //TIOC4A(P4_4) :Left-motor 00755 //TIOC4B(P4_5) :Right-motor 00756 //------------------------------------------------------------------// 00757 void init_MTU2_PWM_Motor( void ) 00758 { 00759 /* Port setting for S/W I/O Contorol */ 00760 /* alternative mode */ 00761 00762 /* MTU2_4 (P4_4)(P4_5) */ 00763 GPIOPBDC4 = 0x0000; /* Bidirection mode disabled*/ 00764 GPIOPFCAE4 &= 0xffcf; /* The alternative function of a pin */ 00765 GPIOPFCE4 |= 0x0030; /* The alternative function of a pin */ 00766 GPIOPFC4 &= 0xffcf; /* The alternative function of a pin */ 00767 /* 2nd altemative function/output */ 00768 GPIOP4 &= 0xffcf; /* */ 00769 GPIOPM4 &= 0xffcf; /* p4_4,P4_5:output */ 00770 GPIOPMC4 |= 0x0030; /* P4_4,P4_5:double */ 00771 00772 /* Mosule stop 33(MTU2) canceling */ 00773 CPGSTBCR3 &= 0xf7; 00774 00775 /* MTU2_3 and MTU2_4 (Motor PWM) */ 00776 MTU2TCR_3 = 0x20; /* TCNT Clear(TGRA), P0φ/1 */ 00777 MTU2TOCR1 = 0x04; /* */ 00778 MTU2TOCR2 = 0x40; /* N L>H P H>L */ 00779 MTU2TMDR_3 = 0x38; /* Buff:ON Reset-Synchronized PWM mode */ 00780 MTU2TMDR_4 = 0x30; /* Buff:ON */ 00781 MTU2TOER = 0xc6; /* TIOC3B,4A,4B enabled output */ 00782 MTU2TCNT_3 = MTU2TCNT_4 = 0; /* TCNT3,TCNT4 Set 0 */ 00783 MTU2TGRA_3 = MTU2TGRC_3 = MOTOR_PWM_CYCLE; 00784 /* PWM-Cycle(1ms) */ 00785 MTU2TGRA_4 = MTU2TGRC_4 = 0; /* Left-motor(P4_4) */ 00786 MTU2TGRB_4 = MTU2TGRD_4 = 0; /* Right-motor(P4_5) */ 00787 MTU2TSTR |= 0x40; /* TCNT_4 Start */ 00788 } 00789 00790 //Initialize MTU2 PWM functions 00791 //------------------------------------------------------------------// 00792 //MTU2_0 00793 //PWM mode 1 00794 //TIOC0A(P4_0) :Servo-motor 00795 //------------------------------------------------------------------// 00796 void init_MTU2_PWM_Servo( void ) 00797 { 00798 /* Port setting for S/W I/O Contorol */ 00799 /* alternative mode */ 00800 00801 /* MTU2_0 (P4_0) */ 00802 GPIOPBDC4 = 0x0000; /* Bidirection mode disabled*/ 00803 GPIOPFCAE4 &= 0xfffe; /* The alternative function of a pin */ 00804 GPIOPFCE4 &= 0xfffe; /* The alternative function of a pin */ 00805 GPIOPFC4 |= 0x0001; /* The alternative function of a pin */ 00806 /* 2nd alternative function/output */ 00807 GPIOP4 &= 0xfffe; /* */ 00808 GPIOPM4 &= 0xfffe; /* p4_0:output */ 00809 GPIOPMC4 |= 0x0001; /* P4_0:double */ 00810 00811 /* Mosule stop 33(MTU2) canceling */ 00812 CPGSTBCR3 &= 0xf7; 00813 00814 /* MTU2_0 (Motor PWM) */ 00815 MTU2TCR_0 = 0x22; /* TCNT Clear(TGRA), P0φ/16 */ 00816 MTU2TIORH_0 = 0x52; /* TGRA L>H, TGRB H>L */ 00817 MTU2TMDR_0 = 0x32; /* TGRC and TGRD = Buff-mode*/ 00818 /* PWM-mode1 */ 00819 MTU2TCNT_0 = 0; /* TCNT0 Set 0 */ 00820 MTU2TGRA_0 = MTU2TGRC_0 = SERVO_PWM_CYCLE; 00821 /* PWM-Cycle(16ms) */ 00822 MTU2TGRB_0 = MTU2TGRD_0 = 0; /* Servo-motor(P4_0) */ 00823 MTU2TSTR |= 0x01; /* TCNT_0 Start */ 00824 } 00825 00826 //******************************************************************// 00827 // @brief Interrupt callback function 00828 // @param[in] int_type : VDC5 interrupt type 00829 // @retval None 00830 //*******************************************************************/ 00831 static void IntCallbackFunc_Vfield(DisplayBase::int_type_t int_type) 00832 { 00833 if (vfield_count > 0) { 00834 vfield_count--; 00835 } 00836 /* top or bottom (Change) */ 00837 if ( vfield_count2 == 0 ) vfield_count2 = 1; 00838 else if ( vfield_count2 == 1 ) vfield_count2 = 0; 00839 led_m_user( vfield_count2 ); 00840 } 00841 00842 //******************************************************************// 00843 // @brief Wait for the specified number of times Vsync occurs 00844 // @param[in] wait_count : Wait count 00845 // @retval None 00846 //*******************************************************************/ 00847 static void WaitVfield(const int32_t wait_count) 00848 { 00849 vfield_count = wait_count; 00850 while (vfield_count > 0) { 00851 /* Do nothing */ 00852 } 00853 } 00854 00855 //******************************************************************// 00856 // @brief Interrupt callback function for Vsync interruption 00857 // @param[in] int_type : VDC5 interrupt type 00858 // @retval None 00859 //*******************************************************************/ 00860 static void IntCallbackFunc_Vsync(DisplayBase::int_type_t int_type) 00861 { 00862 if (vsync_count > 0) { 00863 vsync_count--; 00864 } 00865 } 00866 00867 //******************************************************************// 00868 // @brief Wait for the specified number of times Vsync occurs 00869 // @param[in] wait_count : Wait count 00870 // @retval None 00871 //*******************************************************************/ 00872 static void WaitVsync(const int32_t wait_count) 00873 { 00874 vsync_count = wait_count; 00875 while (vsync_count > 0) { 00876 /* Do nothing */ 00877 } 00878 } 00879 00880 //******************************************************************// 00881 // Interrupt function( intTimer ) 00882 //*******************************************************************/ 00883 void intTimer( void ) 00884 { 00885 static int counter = 0; 00886 00887 cnt0++; 00888 cnt1++; 00889 00890 /* field check */ 00891 if( vfield_count2 == vfield_count2_buff ) { 00892 vfield_count2_buff = vfield_count2; 00893 } 00894 /* Top field / bottom field */ 00895 switch( counter++ ) { 00896 case 0: 00897 Image_Extraction( write_buff_addr, ImageData_A, vfield_count2 ); 00898 break; 00899 case 1: 00900 Image_Extraction( write_buff_addr, ImageData_A, vfield_count2 ); 00901 break; 00902 case 2: 00903 Image_Reduction( ImageData_A, 160, ImageComp_A, 8 ); 00904 break; 00905 case 3: 00906 Image_Reduction( ImageData_A, 160, ImageComp_A, 8 ); 00907 break; 00908 case 4: 00909 Binarization_process( ImageComp_A, ImageBinary, 20*15, 128 ); 00910 break; 00911 case 5: 00912 /* Trace by image processing */ 00913 digital_sensor_process( ImageBinary ); 00914 break; 00915 case 6: 00916 //MarkCheck_Triangle 00917 MarkDetect_process_T(); 00918 break; 00919 case 7: 00920 //MarkCheck_Crossline 00921 MarkDetect_process_C(); 00922 break; 00923 case 8: 00924 //MarkCheck_Rightline 00925 MarkDetect_process_R(); 00926 break; 00927 case 9: 00928 //MarkCheck_Leftline 00929 MarkDetect_process_L(); 00930 break; 00931 case 15: 00932 counter = 0; 00933 break; 00934 default: 00935 break; 00936 00937 } 00938 /* LED processing */ 00939 led_m_process(); 00940 } 00941 00942 //******************************************************************// 00943 // functions ( on GR-PEACH board ) 00944 //*******************************************************************/ 00945 //led_rgb Function 00946 //------------------------------------------------------------------// 00947 void led_rgb(int led) 00948 { 00949 LED_R = led & 0x1; 00950 LED_G = (led >> 1 ) & 0x1; 00951 LED_B = (led >> 2 ) & 0x1; 00952 } 00953 00954 //user_button_get Function 00955 //------------------------------------------------------------------// 00956 unsigned int user_button_get( void ) 00957 { 00958 return (~user_botton) & 0x1; /* Read ports with switches */ 00959 } 00960 00961 //led_m_user Function 00962 //------------------------------------------------------------------// 00963 void led_m_user( int led ) 00964 { 00965 USER_LED = led & 0x01; 00966 } 00967 00968 //Lled_m_set Function 00969 //------------------------------------------------------------------// 00970 void led_m_set( int set ) 00971 { 00972 led_set = set; 00973 } 00974 00975 //led_m_process Function for only interrupt 00976 //------------------------------------------------------------------// 00977 void led_m_process( void ) 00978 { 00979 static unsigned long led_timer; 00980 00981 led_timer++; 00982 00983 /* Display */ 00984 if( led_timer < led_data[led_set][1] ) led_rgb( led_data[led_set][0] ); 00985 else if( led_timer < ( led_data[led_set][1] + led_data[led_set][2] ) ) led_rgb( LED_OFF ); 00986 else led_timer = 0; 00987 } 00988 00989 //******************************************************************// 00990 // functions ( on Motor drive board ) 00991 //*******************************************************************/ 00992 //led_out Function 00993 //------------------------------------------------------------------// 00994 void led_out(int led) 00995 { 00996 led = ~led; 00997 LED_3 = led & 0x1; 00998 LED_2 = ( led >> 1 ) & 0x1; 00999 } 01000 01001 //pushsw_get Function 01002 //------------------------------------------------------------------// 01003 unsigned int pushsw_get( void ) 01004 { 01005 return (~push_sw) & 0x1; /* Read ports with switches */ 01006 } 01007 01008 //motor speed control(PWM) 01009 //Arguments: motor:-100 to 100 01010 //Here, 0 is stop, 100 is forward, -100 is reverse 01011 //------------------------------------------------------------------// 01012 void motor( int accele_l, int accele_r ) 01013 { 01014 accele_l = ( accele_l * MAX_SPEED ) / 100; 01015 accele_r = ( accele_r * MAX_SPEED ) / 100; 01016 01017 /* Left Motor Control */ 01018 if( accele_l >= 0 ) { 01019 /* forward */ 01020 Left_motor_signal = 0; 01021 MTU2TGRC_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * accele_l / 100; 01022 } else { 01023 /* reverse */ 01024 Left_motor_signal = 1; 01025 MTU2TGRC_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * ( -accele_l ) / 100; 01026 } 01027 01028 /* Right Motor Control */ 01029 if( accele_r >= 0 ) { 01030 /* forward */ 01031 Right_motor_signal = 0; 01032 MTU2TGRD_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * accele_r / 100; 01033 } else { 01034 /* reverse */ 01035 Right_motor_signal = 1; 01036 MTU2TGRD_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * ( -accele_r ) / 100; 01037 } 01038 } 01039 01040 //motor speed control(PWM) 01041 //Arguments: motor:-100 to 100 01042 //Here, 0 is stop, 100 is forward, -100 is reverse 01043 //------------------------------------------------------------------// 01044 void motor2( int accele_l, int accele_r ) 01045 { 01046 /* Left Motor Control */ 01047 if( accele_l >= 0 ) { 01048 /* forward */ 01049 Left_motor_signal = 0; 01050 MTU2TGRC_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * accele_l / 100; 01051 } else { 01052 /* reverse */ 01053 Left_motor_signal = 1; 01054 MTU2TGRC_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * ( -accele_l ) / 100; 01055 } 01056 01057 /* Right Motor Control */ 01058 if( accele_r >= 0 ) { 01059 /* forward */ 01060 Right_motor_signal = 0; 01061 MTU2TGRD_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * accele_r / 100; 01062 } else { 01063 /* reverse */ 01064 Right_motor_signal = 1; 01065 MTU2TGRD_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * ( -accele_r ) / 100; 01066 } 01067 } 01068 01069 //handle Function 01070 //------------------------------------------------------------------// 01071 void handle( int angle ) 01072 { 01073 handle_buff = angle; 01074 /* When the servo move from left to right in reverse, replace "-" with "+" */ 01075 MTU2TGRD_0 = SERVO_CENTER - angle * HANDLE_STEP; 01076 } 01077 01078 //diff Function 01079 //------------------------------------------------------------------// 01080 int diff( int pwm ) 01081 { 01082 int i, ret; 01083 01084 i = handle_buff; 01085 if( i < 0 ) i = -i; 01086 if( i > 45 ) i = 45; 01087 ret = revolution_difference[i] * pwm / 100; 01088 01089 return ret; 01090 } 01091 01092 //******************************************************************// 01093 // Image process functions 01094 //*******************************************************************/ 01095 //Image Data YCbCr -> Y(320*240pix) -> Y(160*120) 01096 //frame 0 : Top field 01097 //frame 1 : Bottom field 01098 //------------------------------------------------------------------// 01099 void Image_Extraction( unsigned char *buff_addr, unsigned char *Data_Y, int frame ) 01100 { 01101 static int Xp, Yp, inc, Data_Y_buff; 01102 static int counter = 0; 01103 01104 // Distributed processing 01105 switch( counter++ ) { 01106 case 0: 01107 for( Yp = frame, inc = 0; Yp < 120; Yp+=2 ){ 01108 for( Xp = 0; Xp < 640; Xp+=4, inc++ ){ 01109 Data_Y_buff = (int)buff_addr[(Xp+0)+(640*Yp)]; 01110 Data_Y_buff += (int)buff_addr[(Xp+2)+(640*Yp)]; 01111 Data_Y[inc] = Data_Y_buff >> 1; 01112 } 01113 } 01114 break; 01115 case 1: 01116 for( /* None */ ; Yp < 240; Yp+=2 ){ 01117 for( Xp = 0; Xp < 640; Xp+=4, inc++ ){ 01118 Data_Y_buff = (int)buff_addr[(Xp+0)+(640*Yp)]; 01119 Data_Y_buff += (int)buff_addr[(Xp+2)+(640*Yp)]; 01120 Data_Y[inc] = Data_Y_buff >> 1; 01121 } 01122 } 01123 counter = 0; 01124 break; 01125 default: 01126 break; 01127 } 01128 } 01129 01130 //Image_Reduction Function ( Averaging processing ) 01131 //------------------------------------------------------------------// 01132 void Image_Reduction( unsigned char *Data_Y, int Data_W , unsigned char *Comp_Y, int Comp_M ) 01133 { 01134 int Data_H, Pixel_T, Pixel_D; 01135 int x, y; 01136 static int Xp, Yp, inc; 01137 static int counter = 0; 01138 01139 Data_H = (Data_W / (double)4) * 3; 01140 Pixel_D = Comp_M * Comp_M; 01141 01142 switch( counter++ ) { 01143 case 0: 01144 for( Yp = 0, inc = 0; Yp < ( Data_H / 2); Yp+=Comp_M ){ 01145 for( Xp = 0; Xp < Data_W; Xp+=Comp_M, inc++ ){ 01146 Pixel_T = 0; 01147 for( y = 0; y < Comp_M; y++ ){ 01148 for( x = 0; x < Comp_M; x++ ){ 01149 Pixel_T += Data_Y[( Xp + x ) + (( Yp + y ) * Data_W )]; 01150 } 01151 } 01152 Comp_Y[inc] = Pixel_T / Pixel_D; 01153 } 01154 } 01155 break; 01156 case 1: 01157 for( /* None */ ; Yp < Data_H ; Yp+=Comp_M ){ 01158 for( Xp = 0; Xp < Data_W; Xp+=Comp_M, inc++ ){ 01159 Pixel_T = 0; 01160 for( y = 0; y < Comp_M; y++ ){ 01161 for( x = 0; x < Comp_M; x++ ){ 01162 Pixel_T += Data_Y[( Xp + x ) + (( Yp + y ) * Data_W )]; 01163 } 01164 } 01165 Comp_Y[inc] = Pixel_T / Pixel_D; 01166 } 01167 } 01168 counter = 0; 01169 break; 01170 default: 01171 break; 01172 } 01173 } 01174 01175 // Binarization_process Function 01176 //------------------------------------------------------------------// 01177 void Binarization_process( unsigned char *Comp_Y, unsigned char *Binary, long items, int threshold ) 01178 { 01179 int i; 01180 01181 for( i = 0; i < items; i++ ) { 01182 if( Comp_Y[i] >= threshold ) Binary[i] = 1; 01183 else Binary[i] = 0; 01184 } 01185 } 01186 01187 //******************************************************************// 01188 // digital sensor functions 01189 //*******************************************************************/ 01190 //CenterLine_Corrective Function ( image size 20*15pix ) 01191 //------------------------------------------------------------------// 01192 int CenterLine_Corrective( unsigned char *Binary ) 01193 { 01194 #define L 0 01195 #define R 1 01196 01197 int iRet, offset_X, offset_Y; 01198 int Xpix, X; 01199 int Ypix; 01200 int Pixel_diff[2]; 01201 int Error_cnt; 01202 int value; 01203 01204 /* Center of image */ 01205 offset_X = 6; 01206 offset_Y = 12; 01207 01208 /* corrective of center line */ 01209 for( Ypix = 0, Error_cnt = 0; Ypix < (offset_Y - 4); Ypix++ ) { 01210 for( value = 0; value < 2; value++ ) { 01211 for( Xpix = offset_X; Xpix < (offset_X + 8); Xpix++ ) { 01212 /* Lift side */ 01213 Pixel_diff[L] = 0; 01214 if( Binary[ ( ( offset_Y - Ypix ) * 20 ) + Xpix ] >= 1 ) { 01215 for( X = Xpix; X > (Xpix - 4); X-- ) { 01216 if( Binary[ ( ( offset_Y - Ypix ) * 20 ) + X ] >= 1 ) { 01217 Pixel_diff[L]++; 01218 } else { 01219 break; 01220 } 01221 } 01222 } else { 01223 Pixel_diff[L] = -1; 01224 } 01225 /* Right side */ 01226 Pixel_diff[R] = 0; 01227 if( Binary[ ( ( offset_Y - Ypix ) * 20 ) + (Xpix + 1) ] >= 1 ) { 01228 for( X = (Xpix + 1); X < ((Xpix + 1) + 4); X++ ) { 01229 if( Binary[ ( ( offset_Y - Ypix ) * 20 ) + X ] >= 1 ) { 01230 Pixel_diff[R]++; 01231 } else { 01232 break; 01233 } 01234 } 01235 } else { 01236 Pixel_diff[R] = 1; 01237 } 01238 /* check */ 01239 iRet = Pixel_diff[L] - Pixel_diff[R]; 01240 if( value >= iRet && iRet >= -value ) { 01241 break; 01242 } 01243 } 01244 if( value >= iRet && iRet >= -value ) { 01245 /* X coordinate */ 01246 Sensor_X[Ypix][2] = Xpix; 01247 Sensor_X[Ypix][3] = Xpix + 1; 01248 break; 01249 } else { 01250 Sensor_X[Ypix][2] = Sensor_X[Ypix][3] = -1; 01251 Error_cnt++; 01252 } 01253 } 01254 /* Left side sensor */ 01255 Sensor_X[Ypix][1] = Sensor_X[Ypix][2] - ( Pixel_diff[L] ); 01256 Sensor_X[Ypix][0] = Sensor_X[Ypix][1] - ( Pixel_diff[L] + 1 );//( Sensor_X[Ypix][2] - Sensor_X[Ypix][1] ); 01257 /* Right side sensor */ 01258 Sensor_X[Ypix][4] = Sensor_X[Ypix][3] + ( Pixel_diff[R] ); 01259 Sensor_X[Ypix][5] = Sensor_X[Ypix][4] + ( Pixel_diff[R] + 1 );//( Sensor_X[Ypix][4] - Sensor_X[Ypix][3] ); 01260 } 01261 return Error_cnt; 01262 } 01263 01264 //digital_sensor_process Function 01265 //------------------------------------------------------------------// 01266 void digital_sensor_process( unsigned char *Binary ) 01267 { 01268 int Ypix; 01269 int offset_Y; 01270 unsigned char sensor, data; 01271 01272 offset_Y = 12; 01273 sensor = 0; 01274 01275 data = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][2] ] & 0x01; 01276 data |= Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][3] ] & 0x01; 01277 sensor |= (data << 4) & 0x10; 01278 data = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][0] ] & 0x01; 01279 sensor |= (data << 3) & 0x08; 01280 data = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][1] ] & 0x01; 01281 sensor |= (data << 2) & 0x04; 01282 data = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][4] ] & 0x01; 01283 sensor |= (data << 1) & 0x02; 01284 data = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][5] ] & 0x01; 01285 sensor |= (data << 0) & 0x01; 01286 sensor &= 0x1f; 01287 sensor_value = sensor; 01288 } 01289 01290 //digital_sensor Function 01291 //------------------------------------------------------------------// 01292 unsigned char digital_sensor( void ) 01293 { 01294 return sensor_value; 01295 } 01296 01297 //******************************************************************// 01298 // Mark detect functions 01299 //*******************************************************************/ 01300 // Extract_Image 01301 //------------------------------------------------------------------// 01302 void Image_part_Extraction( unsigned char *Binary, int Width, int Xpix, int Ypix, unsigned char *Data_B, int x_size, int y_size ) 01303 { 01304 int x, y; 01305 for( y = 0; y < y_size; y++ ) { 01306 for( x = 0; x < x_size; x++ ) { 01307 Data_B[ x + ( y * x_size ) ] = Binary[ (Xpix + x) + ( (Ypix + y) * Width ) ]; 01308 } 01309 } 01310 } 01311 01312 // Standard deviation 01313 //------------------------------------------------------------------// 01314 double Standard_Deviation( unsigned char *data, double *Devi, int items ) 01315 { 01316 int i; 01317 double iRet_A, iRet_C, iRet_D; 01318 01319 /* A 合計値 平均化 */ 01320 iRet_A = 0; 01321 for( i = 0; i < items; i++ ) { 01322 iRet_A += data[i]; 01323 } 01324 iRet_A /= items; 01325 01326 /* B 偏差値 */ 01327 for( i = 0; i < items; i++ ) { 01328 Devi[i] = data[i] - iRet_A; 01329 } 01330 01331 /* C 分散 */ 01332 iRet_C = 0; 01333 for( i = 0; i < items; i++ ) { 01334 iRet_C += ( Devi[i] * Devi[i] ); 01335 } 01336 iRet_C /= items; 01337 01338 /* D 標準偏差 */ 01339 iRet_D = sqrt( iRet_C ); 01340 01341 return iRet_D; 01342 } 01343 01344 // Covariance 01345 //------------------------------------------------------------------// 01346 double Covariance( double *Devi_A, double *Devi_B, int items ) 01347 { 01348 int i; 01349 double iRet, iRet_buff; 01350 01351 iRet = 0; 01352 for( i = 0; i < items; i++ ) { 01353 iRet_buff = Devi_A[i] * Devi_B[i]; 01354 iRet += iRet_buff; 01355 } 01356 iRet /= items; 01357 01358 return iRet; 01359 } 01360 01361 // Judgement_ImageMatching 01362 //------------------------------------------------------------------// 01363 int Judgement_ImageMatching( double covari, double SDevi_A, double SDevi_B ) 01364 { 01365 int iRet; 01366 01367 iRet = ( covari * 100 ) / ( SDevi_A * SDevi_B ); 01368 01369 return iRet; 01370 } 01371 01372 // MarkDetect_process_T 01373 //------------------------------------------------------------------// 01374 void MarkDetect_process_T( void ) 01375 { 01376 int x, y; 01377 01378 retJudgeIM_Max[0] = 0; 01379 for( y = 0; y <= 12; y++ ) { 01380 for( x = 0; x <= 15; x++ ) { 01381 Image_part_Extraction( ImageBinary, 20, x, y, NowImageBinary, 5, 3 ); 01382 retDevi = Standard_Deviation( NowImageBinary, NowDevi, 15 ); 01383 retCovari = Covariance( TempDevi_Triangle, NowDevi, 15 ); 01384 retJudgeIM = 0; 01385 retJudgeIM = Judgement_ImageMatching( retCovari, retDevi_Triangle, retDevi ); 01386 if( 100 >= retJudgeIM && retJudgeIM > retJudgeIM_Max[0] ) { 01387 Xt = x; 01388 Yt = y; 01389 retJudgeIM_Max[0] = retJudgeIM; 01390 } 01391 } 01392 } 01393 } 01394 01395 // MarkDetect_process_C 01396 //------------------------------------------------------------------// 01397 void MarkDetect_process_C( void ) 01398 { 01399 int x, y; 01400 01401 retJudgeIM_Max[1] = 0; 01402 for( y = 0; y <= 13; y++ ) { 01403 for( x = 0; x <= 6; x++ ) { 01404 Image_part_Extraction( ImageBinary, 20, x, y, NowImageBinary, 14, 2 ); 01405 retDevi = Standard_Deviation( NowImageBinary, NowDevi, 28 ); 01406 retCovari = Covariance( TempDevi_Crossline, NowDevi, 28 ); 01407 retJudgeIM = 0; 01408 retJudgeIM = Judgement_ImageMatching( retCovari, retDevi_Crossline, retDevi ); 01409 if( 100 >= retJudgeIM && retJudgeIM > retJudgeIM_Max[1] ) { 01410 Xc = x; 01411 Yc = y; 01412 retJudgeIM_Max[1] = retJudgeIM; 01413 } 01414 } 01415 } 01416 } 01417 01418 // MarkDetect_process_R 01419 //------------------------------------------------------------------// 01420 void MarkDetect_process_R( void ) 01421 { 01422 int x, y; 01423 01424 retJudgeIM_Max[2] = 0; 01425 for( y = 0; y <= 13; y++ ) { 01426 for( x = 0; x <= 6; x++ ) { 01427 Image_part_Extraction( ImageBinary, 20, x, y, NowImageBinary, 14, 2 ); 01428 retDevi = Standard_Deviation( NowImageBinary, NowDevi, 28 ); 01429 retCovari = Covariance( TempDevi_Rightline, NowDevi, 28 ); 01430 retJudgeIM = 0; 01431 retJudgeIM = Judgement_ImageMatching( retCovari, retDevi_Rightline, retDevi ); 01432 if( 100 >= retJudgeIM && retJudgeIM > retJudgeIM_Max[2] ) { 01433 Xr = x; 01434 Yr = y; 01435 retJudgeIM_Max[2] = retJudgeIM; 01436 } 01437 } 01438 } 01439 } 01440 01441 // MarkDetect_process_L 01442 //------------------------------------------------------------------// 01443 void MarkDetect_process_L( void ) 01444 { 01445 int x, y; 01446 01447 retJudgeIM_Max[3] = 0; 01448 for( y = 0; y <= 13; y++ ) { 01449 for( x = 0; x <= 6; x++ ) { 01450 Image_part_Extraction( ImageBinary, 20, x, y, NowImageBinary, 14, 2 ); 01451 retDevi = Standard_Deviation( NowImageBinary, NowDevi, 28 ); 01452 retCovari = Covariance( TempDevi_Leftline, NowDevi, 28 ); 01453 retJudgeIM = 0; 01454 retJudgeIM = Judgement_ImageMatching( retCovari, retDevi_Leftline, retDevi ); 01455 if( 100 >= retJudgeIM && retJudgeIM > retJudgeIM_Max[3] ) { 01456 Xl = x; 01457 Yl = y; 01458 retJudgeIM_Max[3] = retJudgeIM; 01459 } 01460 } 01461 } 01462 } 01463 01464 // MarkCheck Triangle detection 01465 // Return values: 0: no triangle mark, 1: Triangle mark 01466 //------------------------------------------------------------------// 01467 int MarkCheck_Triangle( int percentage ) 01468 { 01469 int ret; 01470 01471 ret = 0; 01472 if( retJudgeIM_Max[0] >= percentage ) { 01473 ret = 1; 01474 } 01475 01476 return ret; 01477 } 01478 01479 // Cross line detection processing 01480 // Return values: 0: no cross line, 1: cross line 01481 //------------------------------------------------------------------// 01482 int MarkCheck_Crossline( int percentage ) 01483 { 01484 int ret; 01485 01486 ret = 0; 01487 if( retJudgeIM_Max[1] >= percentage ) { 01488 ret = 1; 01489 } 01490 01491 return ret; 01492 } 01493 01494 // Right half line detection processing 01495 // Return values: 0: not detected, 1: detected 01496 //------------------------------------------------------------------// 01497 int MarkCheck_Rightline( int percentage ) 01498 { 01499 int ret; 01500 01501 ret = 0; 01502 if( retJudgeIM_Max[2] >= percentage ) { 01503 ret = 1; 01504 } 01505 01506 return ret; 01507 } 01508 01509 // Left half line detection processing 01510 // Return values: 0: not detected, 1: detected 01511 //------------------------------------------------------------------// 01512 int MarkCheck_Leftline( int percentage ) 01513 { 01514 int ret; 01515 01516 ret = 0; 01517 if( retJudgeIM_Max[3] >= percentage ) { 01518 ret = 1; 01519 } 01520 01521 return ret; 01522 } 01523 01524 //******************************************************************// 01525 // Debug functions 01526 //*******************************************************************/ 01527 //Image Data Output( for the Excel ) 01528 //------------------------------------------------------------------// 01529 void ImageData_Serial_Out( unsigned char *Data_Y, int Width ) 01530 { 01531 int Xp, Yp, inc, Height; 01532 01533 Height = (Width / (double)4) * 3; 01534 for( Yp = 0, inc = 0; Yp < Height; Yp++ ) { 01535 for( Xp = 0; Xp < Width; Xp++, inc++ ) { 01536 pc.printf( "%d,", Data_Y[ inc ] ); 01537 } 01538 pc.printf("\n\r"); 01539 } 01540 } 01541 01542 //Image Data Output2( for TeraTerm ) 01543 //------------------------------------------------------------------// 01544 void ImageData_Serial_Out2( unsigned char *Data_Y, int Width ) 01545 { 01546 int Xp, Yp, Height; 01547 01548 Height = (Width / (double)4) * 3; 01549 for( Yp = 0; Yp < Height; Yp++ ) { 01550 for( Xp = 0; Xp < Width; Xp++ ) { 01551 pc.printf( "%d ", Data_Y[Xp + (Yp * Width)] ); 01552 } 01553 pc.printf( "\n\r" ); 01554 } 01555 01556 //Add display 01557 pc.printf( "\n\r" ); 01558 pc.printf( "sensor_inp = 0x%02x\n\r", digital_sensor() ); 01559 pc.printf( "T = %3d%% %01d X=%2d Y=%2d\n\r", retJudgeIM_Max[0], MarkCheck_Triangle( 90 ), Xt, Yt ); 01560 pc.printf( "C = %3d%% %01d X=%2d Y=%2d\n\r", retJudgeIM_Max[1], MarkCheck_Crossline( 90 ), Xc, Yc ); 01561 pc.printf( "R = %3d%% %01d X=%2d Y=%2d\n\r", retJudgeIM_Max[2], MarkCheck_Rightline( 90 ), Xr, Yr ); 01562 pc.printf( "L = %3d%% %01d X=%2d Y=%2d\n\r", retJudgeIM_Max[3], MarkCheck_Leftline( 90 ), Xl, Yl ); 01563 pc.printf( "\n\r" ); 01564 Height += 7; 01565 01566 pc.printf( "\033[%dA" , Height ); 01567 } 01568 01569 //------------------------------------------------------------------// 01570 // End of file 01571 //------------------------------------------------------------------//
Generated on Thu Jul 14 2022 06:33:53 by
1.7.2