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