Micon Car Rally
This program is line trace program by image processing(NTSC 60fps).
Dependencies: GR-PEACH_video mbed
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 //------------------------------------------------------------------// 00002 //Supported MCU: RZ/A1H 00003 //File Contents: Trace Program by Image Processing 00004 // (GR-PEACH version on the Micon Car) 00005 //Version number: Ver.1.01 00006 //Date: 2017.01.10 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 00055 //Define(NTSC-Video) 00056 //------------------------------------------------------------------// 00057 #define VIDEO_INPUT_CH (DisplayBase::VIDEO_INPUT_CHANNEL_0) 00058 #define VIDEO_INT_TYPE (DisplayBase::INT_TYPE_S0_VFIELD) 00059 #define DATA_SIZE_PER_PIC (2u) 00060 00061 /*! Frame buffer stride: Frame buffer stride should be set to a multiple of 32 or 128 00062 in accordance with the frame buffer burst transfer mode. */ 00063 #define PIXEL_HW (320u) /* QVGA */ 00064 #define PIXEL_VW (240u) /* QVGA */ 00065 #define VIDEO_BUFFER_STRIDE (((PIXEL_HW * DATA_SIZE_PER_PIC) + 31u) & ~31u) 00066 #define VIDEO_BUFFER_HEIGHT (PIXEL_VW) 00067 00068 //Constructor 00069 //------------------------------------------------------------------// 00070 Ticker interrput; 00071 Serial pc(USBTX, USBRX); 00072 DigitalOut LED_R(P6_13); /* LED1 on the GR-PEACH board */ 00073 DigitalOut LED_G(P6_14); /* LED2 on the GR-PEACH board */ 00074 DigitalOut LED_B(P6_15); /* LED3 on the GR-PEACH board */ 00075 DigitalOut USER_LED(P6_12); /* USER_LED on the GR-PEACH board */ 00076 DigitalIn user_botton(P6_0); /* SW1 on the GR-PEACH board */ 00077 00078 DigitalOut Left_motor_signal(P4_6); /* Used by motor fanction */ 00079 DigitalOut Right_motor_signal(P4_7); /* Used by motor fanction */ 00080 DigitalIn push_sw(P2_13); /* SW1 on the Motor Drive board */ 00081 DigitalOut LED_3(P2_14); /* LED3 on the Motor Drive board */ 00082 DigitalOut LED_2(P2_15); /* LED2 on the Motor Drive board */ 00083 00084 //Prototype(NTSC-video) 00085 //------------------------------------------------------------------// 00086 static void IntCallbackFunc_Vfield(DisplayBase::int_type_t int_type); 00087 static void WaitVfield(const int32_t wait_count); 00088 static void IntCallbackFunc_Vsync(DisplayBase::int_type_t int_type); 00089 static void WaitVsync(const int32_t wait_count); 00090 00091 //Prototype 00092 //------------------------------------------------------------------// 00093 //Peripheral functions 00094 void init_MTU2_PWM_Motor( void ); /* Initialize PWM functions */ 00095 void init_MTU2_PWM_Servo( void ); /* Initialize PWM functions */ 00096 void intTimer( void ); /* Interrupt fanction */ 00097 00098 //GR-peach board 00099 void led_rgb(int led); 00100 void led_m_user( int led ); 00101 unsigned int user_button_get( void ); 00102 void led_m_set( int set ); 00103 void led_m_process( void ); /* Function for only interrupt */ 00104 00105 //Motor drive board 00106 void led_out(int led); 00107 unsigned int pushsw_get( void ); 00108 void motor( int accele_l, int accele_r ); 00109 void handle( int angle ); 00110 int diff( int pwm ); 00111 00112 //Prototype(Mark detection) 00113 //------------------------------------------------------------------// 00114 void Image_Extraction( unsigned char *buff_addr, unsigned char *Data_Y, int frame ); 00115 void Image_Reduction( unsigned char *Data_Y, int Data_W , unsigned char *Comp_Y, int Comp_M ); 00116 void Binarization_process( unsigned char *Comp_Y, unsigned char *Binary, long items, int threshold ); 00117 00118 //Prototype(Trace by Image Processing) 00119 //------------------------------------------------------------------// 00120 int CenterLine_Corrective( unsigned char *Binary ); 00121 void digital_sensor_process( unsigned char *Binary ); /* Function for only interrupt */ 00122 unsigned char digital_sensor( void ); 00123 00124 //Prototype(Display Debug) 00125 //------------------------------------------------------------------// 00126 void ImageData_Serial_Out( unsigned char *Data_Y, int Width ); 00127 void ImageData_Serial_Out2( unsigned char *Data_Y, int Width ); 00128 00129 //Globle(NTSC-video) 00130 //------------------------------------------------------------------// 00131 static uint8_t FrameBuffer_Video_A[VIDEO_BUFFER_STRIDE * VIDEO_BUFFER_HEIGHT]__attribute((section("NC_BSS"),aligned(16))); //16 bytes aligned!; 00132 uint8_t * write_buff_addr = FrameBuffer_Video_A; 00133 static volatile int32_t vsync_count; 00134 static volatile int32_t vfield_count; 00135 static volatile int32_t vfield_count2 = 1; 00136 static volatile int32_t vfield_count2_buff; 00137 00138 //Line Detect(Digital sensor) 00139 //------------------------------------------------------------------// 00140 unsigned char ImageData_A[160*120]; 00141 unsigned char ImageComp_A[20*15]; 00142 unsigned char ImageBinary[20*15]; 00143 00144 /* Trace by image processing */ 00145 volatile int Sensor_X[8][6]; 00146 volatile unsigned char sensor_value; 00147 00148 //Globle variable for led fanction 00149 //------------------------------------------------------------------// 00150 volatile int led_set; /* Status */ 00151 00152 // LED, OnTime, OffTime, 00153 volatile int led_data[10][3]= {LED_RED, 50, 50, /* ERROR */ 00154 LED_RED, 500, 0, /* STOP */ 00155 LED_GREEN, 500, 500, /* RUN */ 00156 LED_BLUE, 50, 50, /* DEBUG */ 00157 LED_GREEN, 1, 0, /* MOTOR_START */ 00158 LED_RED, 1, 0}; /* MOTOR_STOP */ 00159 00160 //Globle variable for Trace program 00161 //------------------------------------------------------------------// 00162 volatile unsigned long cnt0; /* Used by timer function */ 00163 volatile unsigned long cnt1; /* Used within main */ 00164 volatile int pattern; /* Pattern numbers */ 00165 volatile int handle_buff; 00166 00167 const int revolution_difference[] = { 00168 100, 98, 97, 95, 93, 00169 92, 90, 88, 87, 85, 00170 84, 82, 81, 79, 78, 00171 76, 75, 73, 72, 71, 00172 69, 68, 66, 65, 64, 00173 62, 61, 59, 58, 57, 00174 55, 54, 52, 51, 50, 00175 48, 47, 45, 44, 42, 00176 41, 39, 38, 36, 35, 00177 33 }; 00178 00179 //******************************************************************// 00180 // Main function 00181 //*******************************************************************/ 00182 int main( void ) 00183 { 00184 /* NTSC-Video */ 00185 DisplayBase::graphics_error_t error; 00186 00187 /* Create DisplayBase object */ 00188 DisplayBase Display; 00189 00190 /* Graphics initialization process */ 00191 error = Display.Graphics_init(NULL); 00192 if (error != DisplayBase::GRAPHICS_OK) { 00193 printf("Line %d, error %d\n", __LINE__, error); 00194 while (1); 00195 } 00196 00197 error = Display.Graphics_Video_init( DisplayBase::INPUT_SEL_VDEC, NULL); 00198 if( error != DisplayBase::GRAPHICS_OK ) { 00199 while(1); 00200 } 00201 00202 /* Interrupt callback function setting (Vsync signal input to scaler 0) */ 00203 error = Display.Graphics_Irq_Handler_Set(DisplayBase::INT_TYPE_S0_VI_VSYNC, 0, IntCallbackFunc_Vsync); 00204 if (error != DisplayBase::GRAPHICS_OK) { 00205 printf("Line %d, error %d\n", __LINE__, error); 00206 while (1); 00207 } 00208 00209 /* Video capture setting (progressive form fixed) */ 00210 error = Display.Video_Write_Setting( 00211 VIDEO_INPUT_CH, 00212 DisplayBase::COL_SYS_NTSC_358, 00213 write_buff_addr, 00214 VIDEO_BUFFER_STRIDE, 00215 DisplayBase::VIDEO_FORMAT_YCBCR422, 00216 DisplayBase::WR_RD_WRSWA_32_16BIT, 00217 PIXEL_VW, 00218 PIXEL_HW 00219 ); 00220 if (error != DisplayBase::GRAPHICS_OK) { 00221 printf("Line %d, error %d\n", __LINE__, error); 00222 while (1); 00223 } 00224 00225 /* Interrupt callback function setting (Field end signal for recording function in scaler 0) */ 00226 error = Display.Graphics_Irq_Handler_Set(VIDEO_INT_TYPE, 0, IntCallbackFunc_Vfield); 00227 if (error != DisplayBase::GRAPHICS_OK) { 00228 printf("Line %d, error %d\n", __LINE__, error); 00229 while (1); 00230 } 00231 00232 /* Video write process start */ 00233 error = Display.Video_Start (VIDEO_INPUT_CH); 00234 if (error != DisplayBase::GRAPHICS_OK) { 00235 printf("Line %d, error %d\n", __LINE__, error); 00236 while (1); 00237 } 00238 00239 /* Video write process stop */ 00240 error = Display.Video_Stop (VIDEO_INPUT_CH); 00241 if (error != DisplayBase::GRAPHICS_OK) { 00242 printf("Line %d, error %d\n", __LINE__, error); 00243 while (1); 00244 } 00245 00246 /* Video write process start */ 00247 error = Display.Video_Start (VIDEO_INPUT_CH); 00248 if (error != DisplayBase::GRAPHICS_OK) { 00249 printf("Line %d, error %d\n", __LINE__, error); 00250 while (1); 00251 } 00252 00253 /* Wait vsync to update resister */ 00254 WaitVsync(1); 00255 00256 /* Wait 2 Vfield(Top or bottom field) */ 00257 WaitVfield(2); 00258 00259 /* Initialize MCU functions */ 00260 init_MTU2_PWM_Motor(); 00261 init_MTU2_PWM_Servo(); 00262 interrput.attach(&intTimer, 0.001); 00263 pc.baud(230400); 00264 00265 /* Initialize Micon Car state */ 00266 led_out( 0x0 ); 00267 handle( 0 ); 00268 motor( 0, 0 ); 00269 00270 /* wait to stabilize NTSC signal (about 170ms) */ 00271 wait(0.2); 00272 00273 /* Initialize digital sensor */ 00274 CenterLine_Corrective( ImageBinary ); 00275 00276 if( user_button_get() ) { 00277 wait(0.1); 00278 led_m_set( DEBUG ); 00279 while( user_button_get() ); 00280 wait(0.5); 00281 pc.printf( "Please push the SW ( on the Motor drive board )\n\r" ); 00282 pc.printf( "\n\r" ); 00283 while( !user_button_get() ); 00284 while( 1 ){ 00285 ImageData_Serial_Out2( ImageBinary, 20 ); 00286 } 00287 } 00288 00289 led_m_set( RUN ); 00290 00291 while( 1 ) { 00292 00293 switch( pattern ) { 00294 /***************************************************************** 00295 About patern 00296 0:wait for switch input 00297 1:check if start bar is open 00298 11:normal trace 00299 12:Left side 00300 13:right side 00301 *****************************************************************/ 00302 case 0: 00303 /* wait for switch input */ 00304 if( pushsw_get() ) { 00305 led_out( 0x0 ); 00306 led_m_set( RUN ); 00307 pattern = 11; 00308 cnt1 = 0; 00309 break; 00310 } 00311 if( cnt1 < 100 ) { 00312 led_out( 0x1 ); 00313 } else if( cnt1 < 200 ) { 00314 led_out( 0x2 ); 00315 } else { 00316 cnt1 = 0; 00317 } 00318 break; 00319 00320 case 11: 00321 /* normal trace */ 00322 switch( (digital_sensor()&0x0f) ) { 00323 case 0x00: 00324 handle( 0 ); 00325 motor( 100, 100 ); 00326 break; 00327 case 0x02: 00328 handle( 3 ); 00329 motor( 100, diff(100) ); 00330 break; 00331 case 0x03: 00332 handle( 12 ); 00333 motor( 100, diff(100) ); 00334 break; 00335 case 0x01: 00336 handle( 20 ); 00337 motor( 100, diff(100) ); 00338 pattern = 12; 00339 break; 00340 case 0x04: 00341 handle( -3 ); 00342 motor( diff(100), 100 ); 00343 break; 00344 case 0x0c: 00345 handle( -12 ); 00346 motor( diff(100), 100 ); 00347 break; 00348 case 0x08: 00349 handle( -20 ); 00350 motor( diff(100), 100 ); 00351 pattern = 13; 00352 break; 00353 default: 00354 break; 00355 } 00356 break; 00357 00358 case 12: 00359 /* Left side */ 00360 if( (digital_sensor()&0x02) == 0x02 ) { 00361 pattern = 11; 00362 break; 00363 } 00364 switch( (digital_sensor()&0x0f) ) { 00365 case 0x01: 00366 handle( 20 ); 00367 motor( 100, diff(100) ); 00368 break; 00369 case 0x00: 00370 case 0x08: 00371 case 0x0c: 00372 handle( 22 ); 00373 motor( 100, diff(100) ); 00374 break; 00375 default: 00376 break; 00377 } 00378 break; 00379 00380 case 13: 00381 /* right side */ 00382 if( (digital_sensor()&0x04) == 0x04 ) { 00383 pattern = 11; 00384 } 00385 switch( (digital_sensor()&0x0f) ) { 00386 case 0x08: 00387 handle( -20 ); 00388 motor( diff(100), 100 ); 00389 break; 00390 case 0x00: 00391 case 0x01: 00392 case 0x03: 00393 handle( -22 ); 00394 motor( diff(100), 100 ); 00395 break; 00396 default: 00397 break; 00398 } 00399 break; 00400 00401 default: 00402 break; 00403 } 00404 } 00405 } 00406 00407 //******************************************************************// 00408 // Initialize functions 00409 //*******************************************************************/ 00410 //Initialize MTU2 PWM functions 00411 //------------------------------------------------------------------// 00412 //MTU2_3, MTU2_4 00413 //Reset-Synchronized PWM mode 00414 //TIOC4A(P4_4) :Left-motor 00415 //TIOC4B(P4_5) :Right-motor 00416 //------------------------------------------------------------------// 00417 void init_MTU2_PWM_Motor( void ) 00418 { 00419 /* Port setting for S/W I/O Contorol */ 00420 /* alternative mode */ 00421 00422 /* MTU2_4 (P4_4)(P4_5) */ 00423 GPIOPBDC4 = 0x0000; /* Bidirection mode disabled*/ 00424 GPIOPFCAE4 &= 0xffcf; /* The alternative function of a pin */ 00425 GPIOPFCE4 |= 0x0030; /* The alternative function of a pin */ 00426 GPIOPFC4 &= 0xffcf; /* The alternative function of a pin */ 00427 /* 2nd altemative function/output */ 00428 GPIOP4 &= 0xffcf; /* */ 00429 GPIOPM4 &= 0xffcf; /* p4_4,P4_5:output */ 00430 GPIOPMC4 |= 0x0030; /* P4_4,P4_5:double */ 00431 00432 /* Mosule stop 33(MTU2) canceling */ 00433 CPGSTBCR3 &= 0xf7; 00434 00435 /* MTU2_3 and MTU2_4 (Motor PWM) */ 00436 MTU2TCR_3 = 0x20; /* TCNT Clear(TGRA), P0φ/1 */ 00437 MTU2TOCR1 = 0x04; /* */ 00438 MTU2TOCR2 = 0x40; /* N L>H P H>L */ 00439 MTU2TMDR_3 = 0x38; /* Buff:ON Reset-Synchronized PWM mode */ 00440 MTU2TMDR_4 = 0x30; /* Buff:ON */ 00441 MTU2TOER = 0xc6; /* TIOC3B,4A,4B enabled output */ 00442 MTU2TCNT_3 = MTU2TCNT_4 = 0; /* TCNT3,TCNT4 Set 0 */ 00443 MTU2TGRA_3 = MTU2TGRC_3 = MOTOR_PWM_CYCLE; 00444 /* PWM-Cycle(1ms) */ 00445 MTU2TGRA_4 = MTU2TGRC_4 = 0; /* Left-motor(P4_4) */ 00446 MTU2TGRB_4 = MTU2TGRD_4 = 0; /* Right-motor(P4_5) */ 00447 MTU2TSTR |= 0x40; /* TCNT_4 Start */ 00448 } 00449 00450 //Initialize MTU2 PWM functions 00451 //------------------------------------------------------------------// 00452 //MTU2_0 00453 //PWM mode 1 00454 //TIOC0A(P4_0) :Servo-motor 00455 //------------------------------------------------------------------// 00456 void init_MTU2_PWM_Servo( void ) 00457 { 00458 /* Port setting for S/W I/O Contorol */ 00459 /* alternative mode */ 00460 00461 /* MTU2_0 (P4_0) */ 00462 GPIOPBDC4 = 0x0000; /* Bidirection mode disabled*/ 00463 GPIOPFCAE4 &= 0xfffe; /* The alternative function of a pin */ 00464 GPIOPFCE4 &= 0xfffe; /* The alternative function of a pin */ 00465 GPIOPFC4 |= 0x0001; /* The alternative function of a pin */ 00466 /* 2nd alternative function/output */ 00467 GPIOP4 &= 0xfffe; /* */ 00468 GPIOPM4 &= 0xfffe; /* p4_0:output */ 00469 GPIOPMC4 |= 0x0001; /* P4_0:double */ 00470 00471 /* Mosule stop 33(MTU2) canceling */ 00472 CPGSTBCR3 &= 0xf7; 00473 00474 /* MTU2_0 (Motor PWM) */ 00475 MTU2TCR_0 = 0x22; /* TCNT Clear(TGRA), P0φ/16 */ 00476 MTU2TIORH_0 = 0x52; /* TGRA L>H, TGRB H>L */ 00477 MTU2TMDR_0 = 0x32; /* TGRC and TGRD = Buff-mode*/ 00478 /* PWM-mode1 */ 00479 MTU2TCNT_0 = 0; /* TCNT0 Set 0 */ 00480 MTU2TGRA_0 = MTU2TGRC_0 = SERVO_PWM_CYCLE; 00481 /* PWM-Cycle(16ms) */ 00482 MTU2TGRB_0 = MTU2TGRD_0 = 0; /* Servo-motor(P4_0) */ 00483 MTU2TSTR |= 0x01; /* TCNT_0 Start */ 00484 } 00485 00486 //******************************************************************// 00487 // @brief Interrupt callback function 00488 // @param[in] int_type : VDC5 interrupt type 00489 // @retval None 00490 //*******************************************************************/ 00491 static void IntCallbackFunc_Vfield(DisplayBase::int_type_t int_type) 00492 { 00493 if (vfield_count > 0) { 00494 vfield_count--; 00495 } 00496 /* top or bottom (Change) */ 00497 if ( vfield_count2 == 0 ) vfield_count2 = 1; 00498 else if ( vfield_count2 == 1 ) vfield_count2 = 0; 00499 } 00500 00501 //******************************************************************// 00502 // @brief Wait for the specified number of times Vsync occurs 00503 // @param[in] wait_count : Wait count 00504 // @retval None 00505 //*******************************************************************/ 00506 static void WaitVfield(const int32_t wait_count) 00507 { 00508 vfield_count = wait_count; 00509 while (vfield_count > 0) { 00510 /* Do nothing */ 00511 } 00512 } 00513 00514 //******************************************************************// 00515 // @brief Interrupt callback function for Vsync interruption 00516 // @param[in] int_type : VDC5 interrupt type 00517 // @retval None 00518 //*******************************************************************/ 00519 static void IntCallbackFunc_Vsync(DisplayBase::int_type_t int_type) 00520 { 00521 if (vsync_count > 0) { 00522 vsync_count--; 00523 } 00524 } 00525 00526 //******************************************************************// 00527 // @brief Wait for the specified number of times Vsync occurs 00528 // @param[in] wait_count : Wait count 00529 // @retval None 00530 //*******************************************************************/ 00531 static void WaitVsync(const int32_t wait_count) 00532 { 00533 vsync_count = wait_count; 00534 while (vsync_count > 0) { 00535 /* Do nothing */ 00536 } 00537 } 00538 00539 //******************************************************************// 00540 // Interrupt function( intTimer ) 00541 //*******************************************************************/ 00542 void intTimer( void ) 00543 { 00544 static int counter = 0; 00545 00546 cnt0++; 00547 cnt1++; 00548 00549 /* field check */ 00550 if( vfield_count2 != vfield_count2_buff ) { 00551 vfield_count2_buff = vfield_count2; 00552 counter = 0; 00553 } 00554 /* Top field / bottom field */ 00555 led_m_user( vfield_count2 ); 00556 switch( counter++ ) { 00557 case 0: 00558 Image_Extraction( write_buff_addr, ImageData_A, vfield_count2 ); 00559 break; 00560 case 1: 00561 Image_Extraction( write_buff_addr, ImageData_A, vfield_count2 ); 00562 break; 00563 case 2: 00564 Image_Reduction( ImageData_A, 160, ImageComp_A, 8 ); 00565 break; 00566 case 3: 00567 Image_Reduction( ImageData_A, 160, ImageComp_A, 8 ); 00568 break; 00569 case 4: 00570 Binarization_process( ImageComp_A, ImageBinary, 20*15, 128 ); 00571 break; 00572 case 5: 00573 /* Trace by image processing */ 00574 digital_sensor_process( ImageBinary ); 00575 break; 00576 default: 00577 break; 00578 } 00579 00580 /* LED processing */ 00581 led_m_process(); 00582 } 00583 00584 //******************************************************************// 00585 // functions ( on GR-PEACH board ) 00586 //*******************************************************************/ 00587 //led_rgb Function 00588 //------------------------------------------------------------------// 00589 void led_rgb(int led) 00590 { 00591 LED_R = led & 0x1; 00592 LED_G = (led >> 1 ) & 0x1; 00593 LED_B = (led >> 2 ) & 0x1; 00594 } 00595 00596 //user_button_get Function 00597 //------------------------------------------------------------------// 00598 unsigned int user_button_get( void ) 00599 { 00600 return (~user_botton) & 0x1; /* Read ports with switches */ 00601 } 00602 00603 //led_m_user Function 00604 //------------------------------------------------------------------// 00605 void led_m_user( int led ) 00606 { 00607 USER_LED = led & 0x01; 00608 } 00609 00610 //Lled_m_set Function 00611 //------------------------------------------------------------------// 00612 void led_m_set( int set ) 00613 { 00614 led_set = set; 00615 } 00616 00617 //led_m_process Function for only interrupt 00618 //------------------------------------------------------------------// 00619 void led_m_process( void ) 00620 { 00621 static unsigned long led_timer; 00622 00623 led_timer++; 00624 00625 /* Display */ 00626 if( led_timer < led_data[led_set][1] ) led_rgb( led_data[led_set][0] ); 00627 else if( led_timer < ( led_data[led_set][1] + led_data[led_set][2] ) ) led_rgb( LED_OFF ); 00628 else led_timer = 0; 00629 } 00630 00631 //******************************************************************// 00632 // functions ( on Motor drive board ) 00633 //*******************************************************************/ 00634 //led_out Function 00635 //------------------------------------------------------------------// 00636 void led_out(int led) 00637 { 00638 led = ~led; 00639 LED_3 = led & 0x1; 00640 LED_2 = ( led >> 1 ) & 0x1; 00641 } 00642 00643 //pushsw_get Function 00644 //------------------------------------------------------------------// 00645 unsigned int pushsw_get( void ) 00646 { 00647 return (~push_sw) & 0x1; /* Read ports with switches */ 00648 } 00649 00650 //motor speed control(PWM) 00651 //Arguments: motor:-100 to 100 00652 //Here, 0 is stop, 100 is forward, -100 is reverse 00653 //------------------------------------------------------------------// 00654 void motor( int accele_l, int accele_r ) 00655 { 00656 accele_l = ( accele_l * MAX_SPEED ) / 100; 00657 accele_r = ( accele_r * MAX_SPEED ) / 100; 00658 00659 /* Left Motor Control */ 00660 if( accele_l >= 0 ) { 00661 /* forward */ 00662 Left_motor_signal = 0; 00663 MTU2TGRC_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * accele_l / 100; 00664 } else { 00665 /* reverse */ 00666 Left_motor_signal = 1; 00667 MTU2TGRC_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * ( -accele_l ) / 100; 00668 } 00669 00670 /* Right Motor Control */ 00671 if( accele_r >= 0 ) { 00672 /* forward */ 00673 Right_motor_signal = 0; 00674 MTU2TGRD_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * accele_r / 100; 00675 } else { 00676 /* reverse */ 00677 Right_motor_signal = 1; 00678 MTU2TGRD_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * ( -accele_r ) / 100; 00679 } 00680 } 00681 00682 //handle Function 00683 //------------------------------------------------------------------// 00684 void handle( int angle ) 00685 { 00686 handle_buff = angle; 00687 /* When the servo move from left to right in reverse, replace "-" with "+" */ 00688 MTU2TGRD_0 = SERVO_CENTER - angle * HANDLE_STEP; 00689 } 00690 00691 //diff Function 00692 //------------------------------------------------------------------// 00693 int diff( int pwm ) 00694 { 00695 int i, ret; 00696 00697 i = handle_buff; 00698 if( i < 0 ) i = -i; 00699 if( i > 45 ) i = 45; 00700 ret = revolution_difference[i] * pwm / 100; 00701 00702 return ret; 00703 } 00704 00705 //******************************************************************// 00706 // Image process functions 00707 //*******************************************************************/ 00708 //Image Data YCbCr -> Y(320*240pix) -> Y(160*120) 00709 //frame 0 : Top field 00710 //frame 1 : Bottom field 00711 //------------------------------------------------------------------// 00712 void Image_Extraction( unsigned char *buff_addr, unsigned char *Data_Y, int frame ) 00713 { 00714 static int Xp, Yp, inc, Data_Y_buff; 00715 static int counter = 0; 00716 00717 // Distributed processing 00718 switch( counter++ ) { 00719 case 0: 00720 for( Yp = frame, inc = 0; Yp < 120; Yp+=2 ){ 00721 for( Xp = 0; Xp < 640; Xp+=4, inc++ ){ 00722 Data_Y_buff = (int)buff_addr[(Xp+0)+(640*Yp)]; 00723 Data_Y_buff += (int)buff_addr[(Xp+2)+(640*Yp)]; 00724 Data_Y[inc] = Data_Y_buff >> 1; 00725 } 00726 } 00727 break; 00728 case 1: 00729 for( /* None */ ; Yp < 240; Yp+=2 ){ 00730 for( Xp = 0; Xp < 640; Xp+=4, inc++ ){ 00731 Data_Y_buff = (int)buff_addr[(Xp+0)+(640*Yp)]; 00732 Data_Y_buff += (int)buff_addr[(Xp+2)+(640*Yp)]; 00733 Data_Y[inc] = Data_Y_buff >> 1; 00734 } 00735 } 00736 counter = 0; 00737 break; 00738 default: 00739 break; 00740 } 00741 } 00742 00743 //Image_Reduction Function ( Averaging processing ) 00744 //------------------------------------------------------------------// 00745 void Image_Reduction( unsigned char *Data_Y, int Data_W , unsigned char *Comp_Y, int Comp_M ) 00746 { 00747 int Data_H, Pixel_T, Pixel_D; 00748 int x, y; 00749 static int Xp, Yp, inc; 00750 static int counter = 0; 00751 00752 Data_H = (Data_W / (double)4) * 3; 00753 Pixel_D = Comp_M * Comp_M; 00754 00755 switch( counter++ ) { 00756 case 0: 00757 for( Yp = 0, inc = 0; Yp < ( Data_H / 2); Yp+=Comp_M ){ 00758 for( Xp = 0; Xp < Data_W; Xp+=Comp_M, inc++ ){ 00759 Pixel_T = 0; 00760 for( y = 0; y < Comp_M; y++ ){ 00761 for( x = 0; x < Comp_M; x++ ){ 00762 Pixel_T += Data_Y[( Xp + x ) + (( Yp + y ) * Data_W )]; 00763 } 00764 } 00765 Comp_Y[inc] = Pixel_T / Pixel_D; 00766 } 00767 } 00768 break; 00769 case 1: 00770 for( /* None */ ; Yp < Data_H ; Yp+=Comp_M ){ 00771 for( Xp = 0; Xp < Data_W; Xp+=Comp_M, inc++ ){ 00772 Pixel_T = 0; 00773 for( y = 0; y < Comp_M; y++ ){ 00774 for( x = 0; x < Comp_M; x++ ){ 00775 Pixel_T += Data_Y[( Xp + x ) + (( Yp + y ) * Data_W )]; 00776 } 00777 } 00778 Comp_Y[inc] = Pixel_T / Pixel_D; 00779 } 00780 } 00781 counter = 0; 00782 break; 00783 default: 00784 break; 00785 } 00786 } 00787 00788 // Binarization_process Function 00789 //------------------------------------------------------------------// 00790 void Binarization_process( unsigned char *Comp_Y, unsigned char *Binary, long items, int threshold ) 00791 { 00792 int i; 00793 00794 for( i = 0; i < items; i++ ) { 00795 if( Comp_Y[i] >= threshold ) Binary[i] = 1; 00796 else Binary[i] = 0; 00797 } 00798 } 00799 00800 //******************************************************************// 00801 // digital sensor functions 00802 //*******************************************************************/ 00803 //CenterLine_Corrective Function ( image size 20*15pix ) 00804 //------------------------------------------------------------------// 00805 int CenterLine_Corrective( unsigned char *Binary ) 00806 { 00807 #define L 0 00808 #define R 1 00809 00810 int iRet, offset_X, offset_Y; 00811 int Xpix, X; 00812 int Ypix; 00813 int Pixel_diff[2]; 00814 int Error_cnt; 00815 int value; 00816 00817 /* Center of image */ 00818 offset_X = 6; 00819 offset_Y = 12; 00820 00821 /* corrective of center line */ 00822 for( Ypix = 0, Error_cnt = 0; Ypix < (offset_Y - 4); Ypix++ ) { 00823 for( value = 0; value < 2; value++ ) { 00824 for( Xpix = offset_X; Xpix < (offset_X + 8); Xpix++ ) { 00825 /* Lift side */ 00826 Pixel_diff[L] = 0; 00827 if( Binary[ ( ( offset_Y - Ypix ) * 20 ) + Xpix ] >= 1 ) { 00828 for( X = Xpix; X > (Xpix - 4); X-- ) { 00829 if( Binary[ ( ( offset_Y - Ypix ) * 20 ) + X ] >= 1 ) { 00830 Pixel_diff[L]++; 00831 } else { 00832 break; 00833 } 00834 } 00835 } else { 00836 Pixel_diff[L] = -1; 00837 } 00838 /* Right side */ 00839 Pixel_diff[R] = 0; 00840 if( Binary[ ( ( offset_Y - Ypix ) * 20 ) + (Xpix + 1) ] >= 1 ) { 00841 for( X = (Xpix + 1); X < ((Xpix + 1) + 4); X++ ) { 00842 if( Binary[ ( ( offset_Y - Ypix ) * 20 ) + X ] >= 1 ) { 00843 Pixel_diff[R]++; 00844 } else { 00845 break; 00846 } 00847 } 00848 } else { 00849 Pixel_diff[R] = 1; 00850 } 00851 /* check */ 00852 iRet = Pixel_diff[L] - Pixel_diff[R]; 00853 if( value >= iRet && iRet >= -value ) { 00854 break; 00855 } 00856 } 00857 if( value >= iRet && iRet >= -value ) { 00858 /* X coordinate */ 00859 Sensor_X[Ypix][2] = Xpix; 00860 Sensor_X[Ypix][3] = Xpix + 1; 00861 break; 00862 } else { 00863 Sensor_X[Ypix][2] = Sensor_X[Ypix][3] = -1; 00864 Error_cnt++; 00865 } 00866 } 00867 /* Left side sensor */ 00868 Sensor_X[Ypix][1] = Sensor_X[Ypix][2] - ( Pixel_diff[L] ); 00869 Sensor_X[Ypix][0] = Sensor_X[Ypix][1] - ( Pixel_diff[L] + 1 );//( Sensor_X[Ypix][2] - Sensor_X[Ypix][1] ); 00870 /* Right side sensor */ 00871 Sensor_X[Ypix][4] = Sensor_X[Ypix][3] + ( Pixel_diff[R] ); 00872 Sensor_X[Ypix][5] = Sensor_X[Ypix][4] + ( Pixel_diff[R] + 1 );//( Sensor_X[Ypix][4] - Sensor_X[Ypix][3] ); 00873 } 00874 return Error_cnt; 00875 } 00876 00877 //digital_sensor_process Function 00878 //------------------------------------------------------------------// 00879 void digital_sensor_process( unsigned char *Binary ) 00880 { 00881 int Ypix; 00882 int offset_Y; 00883 unsigned char sensor, data; 00884 00885 offset_Y = 12; 00886 sensor = 0; 00887 00888 data = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][2] ] & 0x01; 00889 data |= Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][3] ] & 0x01; 00890 sensor |= (data << 4) & 0x10; 00891 data = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][0] ] & 0x01; 00892 sensor |= (data << 3) & 0x08; 00893 data = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][1] ] & 0x01; 00894 sensor |= (data << 2) & 0x04; 00895 data = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][4] ] & 0x01; 00896 sensor |= (data << 1) & 0x02; 00897 data = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][5] ] & 0x01; 00898 sensor |= (data << 0) & 0x01; 00899 sensor &= 0x1f; 00900 sensor_value = sensor; 00901 } 00902 00903 //digital_sensor Function 00904 //------------------------------------------------------------------// 00905 unsigned char digital_sensor( void ) 00906 { 00907 return sensor_value; 00908 } 00909 00910 //******************************************************************// 00911 // Debug functions 00912 //*******************************************************************/ 00913 //Image Data Output( for the Excel ) 00914 //------------------------------------------------------------------// 00915 void ImageData_Serial_Out( unsigned char *Data_Y, int Width ) 00916 { 00917 int Xp, Yp, inc, Height; 00918 00919 Height = (Width / (double)4) * 3; 00920 for( Yp = 0, inc = 0; Yp < Height; Yp++ ) { 00921 for( Xp = 0; Xp < Width; Xp++, inc++ ) { 00922 pc.printf( "%d,", Data_Y[ inc ] ); 00923 } 00924 pc.printf("\n\r"); 00925 } 00926 } 00927 00928 //Image Data Output2( for TeraTerm ) 00929 //------------------------------------------------------------------// 00930 void ImageData_Serial_Out2( unsigned char *Data_Y, int Width ) 00931 { 00932 int Xp, Yp, Height; 00933 00934 Height = (Width / (double)4) * 3; 00935 for( Yp = 0; Yp < Height; Yp++ ) { 00936 for( Xp = 0; Xp < Width; Xp++ ) { 00937 pc.printf( "%d ", Data_Y[Xp + (Yp * Width)] ); 00938 } 00939 pc.printf( "\n\r" ); 00940 } 00941 00942 //Add display 00943 pc.printf( "\n\r" ); 00944 pc.printf( "sensor_inp = 0x%02x\n\r", digital_sensor() ); 00945 pc.printf( "\n\r" ); 00946 Height += 3; 00947 00948 pc.printf( "\033[%dA" , Height ); 00949 } 00950 00951 //------------------------------------------------------------------// 00952 // End of file 00953 //------------------------------------------------------------------//
Generated on Sun Jul 24 2022 15:28:09 by
1.7.2