main.cpp

00001 //------------------------------------------------------------------//
00002 //Supported MCU:   RZ/A1H
00003 //File Contents:   Display Debug
00004 //Version number:  Ver.1.00
00005 //Date:            2016.11.17
00006 //Copyright:       Renesas Electronics Corporation
00007 //                 Hitachi Document Solutions Co., Ltd.
00008 //------------------------------------------------------------------//
00009 
00010 //This program supports the following boards:
00011 //* GR-PEACH(E version)
00012 //* Motor drive board Ver.5
00013 //* Camera module (SC-310)
00014 
00015 //Include
00016 //------------------------------------------------------------------//
00017 #include "mbed.h"
00018 #include "math.h"
00019 #include "iodefine.h"
00020 #include "DisplayBace.h"
00021  
00022 //Define
00023 //------------------------------------------------------------------//
00024 //LED Color on GR-PEACH
00025 #define     LED_OFF             0x00
00026 #define     LED_RED             0x01
00027 #define     LED_GREEN           0x02
00028 #define     LED_YELLOW          0x03
00029 #define     LED_BLUE            0x04
00030 #define     LED_PURPLE          0x05
00031 #define     LED_SKYBLUE         0x06
00032 #define     LED_WHITE           0x07
00033  
00034 //Status
00035 #define     ERROR               0x00
00036 #define     STOP                0x01
00037 #define     RUN                 0x02
00038 #define     DEBUG               0x03
00039 #define     MOTOR_START         0x04
00040 #define     MOTOR_STOP          0x05
00041 #define     MARK_T              0x06
00042  
00043 //Define(NTSC-Video)
00044 //------------------------------------------------------------------//
00045 #define VIDEO_INPUT_CH         (DisplayBase::VIDEO_INPUT_CHANNEL_0)
00046 #define VIDEO_INT_TYPE         (DisplayBase::INT_TYPE_S0_VFIELD)
00047 #define DATA_SIZE_PER_PIC      (2u)
00048  
00049 /*! Frame buffer stride: Frame buffer stride should be set to a multiple of 32 or 128
00050     in accordance with the frame buffer burst transfer mode. */
00051 #define PIXEL_HW               (320u)  /* QVGA */
00052 #define PIXEL_VW               (240u)  /* QVGA */
00053 #define VIDEO_BUFFER_STRIDE    (((PIXEL_HW * DATA_SIZE_PER_PIC) + 31u) & ~31u)
00054 #define VIDEO_BUFFER_HEIGHT    (PIXEL_VW)
00055  
00056 //Constructor
00057 //------------------------------------------------------------------//
00058 Ticker      interrput;
00059 Serial      pc(USBTX, USBRX);
00060 DigitalOut  LED_R(P6_13);               /* LED1 on the GR-PEACH board */
00061 DigitalOut  LED_G(P6_14);               /* LED2 on the GR-PEACH board */
00062 DigitalOut  LED_B(P6_15);               /* LED3 on the GR-PEACH board */
00063 DigitalOut  USER_LED(P6_12);            /* USER_LED on the GR-PEACH board */
00064 DigitalIn   user_botton(P6_0);          /* SW1 on the GR-PEACH board */
00065  
00066 DigitalIn   push_sw(P2_13);             /* SW1 on the Motor Drive board */
00067 DigitalOut  LED_3(P2_14);               /* LED3 on the Motor Drive board */
00068 DigitalOut  LED_2(P2_15);               /* LED2 on the Motor Drive board */
00069 
00070 //Prototype(NTSC-video)
00071 //------------------------------------------------------------------//
00072 static void IntCallbackFunc_Vfield(DisplayBase::int_type_t int_type);
00073 static void WaitVfield(const int32_t wait_count);
00074 static void IntCallbackFunc_Vsync(DisplayBase::int_type_t int_type);
00075 static void WaitVsync(const int32_t wait_count);
00076 
00077 //Prototype
00078 //------------------------------------------------------------------//
00079 //Peripheral functions
00080 void intTimer( void );                  /* Interrupt fanction       */
00081  
00082 //GR-peach board
00083 void led_rgb(int led);
00084 void led_m_user( int led );
00085 unsigned int user_button_get( void );
00086 void led_m_set( int set );
00087 void led_m_process( void );             /* Function for only interrupt */
00088  
00089 //Motor drive board
00090 void led_out(int led);
00091 unsigned int pushsw_get( void );
00092  
00093 //Prototype(Image process)
00094 //------------------------------------------------------------------//
00095 void Image_Extraction( unsigned char *buff_addr, unsigned char *Data_Y, int frame );
00096 void Image_Reduction( unsigned char *Data_Y, int Data_W , unsigned char *Comp_Y, int Comp_M );
00097 void Binarization_process( unsigned char *Comp_Y, unsigned char *Binary, long items, int threshold );
00098 
00099 //Prototype(Mark detection process)
00100 //------------------------------------------------------------------//
00101 void Image_part_Extraction( unsigned char *Binary, int Width, int Xpix, int Ypix, unsigned char *Data_B, int x_size, int y_size );
00102 double Standard_Deviation( unsigned char *data, double *Devi, int items );
00103 double Covariance( double *Devi_A, double *Devi_B, int items );
00104 int Judgement_ImageMatching( double covari, double SDevi_A, double SDevi_B );
00105 void MarkDetect_process_T( void );
00106 int MarkCheck_Triangle( int percentage );
00107 
00108 //Prototype(Display Debug)
00109 //------------------------------------------------------------------//
00110 void ImageData_Serial_Out( unsigned char *Data_Y, int Width );
00111 void ImageData_Serial_Out2( unsigned char *Data_Y, int Width );
00112 
00113 //Globle variable (NTSC-video)
00114 //------------------------------------------------------------------//
00115 static uint8_t FrameBuffer_Video_A[VIDEO_BUFFER_STRIDE * VIDEO_BUFFER_HEIGHT]__attribute((section("NC_BSS"),aligned(16)));  //16 bytes aligned!;
00116 uint8_t * write_buff_addr = FrameBuffer_Video_A;
00117 static volatile int32_t vsync_count;
00118 static volatile int32_t vfield_count;
00119 static volatile int32_t vfield_count2 = 1;
00120 static volatile int32_t vfield_count2_buff;
00121 
00122 //Globle variable for Image process
00123 //------------------------------------------------------------------//
00124 unsigned char   ImageData_A[160*120];
00125 unsigned char   ImageComp_A[20*15];
00126 unsigned char   ImageBinary[20*15];
00127 
00128 //Globle variable for Digital sensor process
00129 //------------------------------------------------------------------//
00130 volatile int            Sensor_X[8][6];
00131 volatile unsigned char  sensor_value;
00132 
00133 //Globle variable for Mark detection process
00134 //------------------------------------------------------------------//
00135 double          TempDevi_Triangle[15];
00136 unsigned char   TempBinary_Triangle[15] = {0,1,1,1,0,
00137                                            0,0,1,0,0,
00138                                            0,0,0,0,0};
00139 
00140 double          NowDevi[15];
00141 unsigned char   NowImageBinary[15];
00142 
00143 volatile double retDevi_Triangle;
00144 
00145 volatile double retDevi;
00146 volatile double retCovari;
00147 volatile int    retJudgeIM;
00148 volatile int    retJudgeIM_Max[1];
00149 
00150 int             Xt, Yt;
00151 
00152 //Globle variable for led fanction
00153 //------------------------------------------------------------------//
00154 volatile int    led_set;                /* Status                   */
00155 
00156                                // LED,  OnTime,  OffTime,
00157 volatile int    led_data[10][3]= {LED_RED,     50,    50,   /* ERROR  */
00158                                   LED_RED,    500,     0,   /* STOP   */
00159                                   LED_GREEN,  500,   500,   /* RUN    */
00160                                   LED_BLUE,    50,    50,   /* DEBUG  */
00161                                   LED_GREEN,    1,     0,   /* MOTOR_START */
00162                                   LED_RED,      1,     0,   /* MOTOR_STOP */
00163                                   LED_WHITE,  500,   500};  /* MARK_T */
00164 
00165 //Globle variable for Trace program
00166 //------------------------------------------------------------------//
00167 volatile unsigned long  cnt0;           /* Used by timer function   */
00168 volatile unsigned long  cnt1;           /* Used within main         */
00169 
00170 //******************************************************************//
00171 // Main function
00172 //*******************************************************************/
00173 int main( void )
00174 {
00175     /* NTSC-Video */
00176     DisplayBase::graphics_error_t error;
00177  
00178     /* Create DisplayBase object */
00179     DisplayBase Display;
00180  
00181     /* Graphics initialization process */
00182     error = Display.Graphics_init(NULL);
00183     if (error != DisplayBase::GRAPHICS_OK) {
00184         printf("Line %d, error %d\n", __LINE__, error);
00185         while (1);
00186     }
00187  
00188     error = Display.Graphics_Video_init( DisplayBase::INPUT_SEL_VDEC, NULL);
00189     if( error != DisplayBase::GRAPHICS_OK ) {
00190         while(1);
00191     }
00192  
00193     /* Interrupt callback function setting (Vsync signal input to scaler 0) */
00194     error = Display.Graphics_Irq_Handler_Set(DisplayBase::INT_TYPE_S0_VI_VSYNC, 0, IntCallbackFunc_Vsync);
00195     if (error != DisplayBase::GRAPHICS_OK) {
00196         printf("Line %d, error %d\n", __LINE__, error);
00197         while (1);
00198     }
00199  
00200     /* Video capture setting (progressive form fixed) */
00201     error = Display.Video_Write_Setting(
00202                 VIDEO_INPUT_CH,
00203                 DisplayBase::COL_SYS_NTSC_358,
00204                 write_buff_addr,
00205                 VIDEO_BUFFER_STRIDE,
00206                 DisplayBase::VIDEO_FORMAT_YCBCR422,
00207                 DisplayBase::WR_RD_WRSWA_32_16BIT,
00208                 PIXEL_VW,
00209                 PIXEL_HW
00210             );
00211     if (error != DisplayBase::GRAPHICS_OK) {
00212         printf("Line %d, error %d\n", __LINE__, error);
00213         while (1);
00214     }
00215  
00216     /* Interrupt callback function setting (Field end signal for recording function in scaler 0) */
00217     error = Display.Graphics_Irq_Handler_Set(VIDEO_INT_TYPE, 0, IntCallbackFunc_Vfield);
00218     if (error != DisplayBase::GRAPHICS_OK) {
00219         printf("Line %d, error %d\n", __LINE__, error);
00220         while (1);
00221     }
00222  
00223     /* Video write process start */
00224     error = Display.Video_Start (VIDEO_INPUT_CH);
00225     if (error != DisplayBase::GRAPHICS_OK) {
00226         printf("Line %d, error %d\n", __LINE__, error);
00227         while (1);
00228     }
00229  
00230     /* Video write process stop */
00231     error = Display.Video_Stop (VIDEO_INPUT_CH);
00232     if (error != DisplayBase::GRAPHICS_OK) {
00233         printf("Line %d, error %d\n", __LINE__, error);
00234         while (1);
00235     }
00236  
00237     /* Video write process start */
00238     error = Display.Video_Start (VIDEO_INPUT_CH);
00239     if (error != DisplayBase::GRAPHICS_OK) {
00240         printf("Line %d, error %d\n", __LINE__, error);
00241         while (1);
00242     }
00243  
00244     /* Wait vsync to update resister */
00245     WaitVsync(1);
00246  
00247     /* Wait 2 Vfield(Top or bottom field) */
00248     WaitVfield(2);
00249  
00250     /* Initialize MCU functions */
00251     interrput.attach(&intTimer, 0.001);
00252     pc.baud(230400);
00253 
00254     /* Initialize Micon Car state */
00255     led_out( 0x0 );
00256  
00257     /* wait to stabilize NTSC signal (about 170ms) */
00258     wait(0.2);
00259 
00260     /* Initialize Mark detection */
00261     retDevi_Triangle  = Standard_Deviation( TempBinary_Triangle, TempDevi_Triangle, 15 );
00262 
00263     wait(0.1);
00264     led_m_set( DEBUG );
00265     pc.printf( "Please push the SW ( on the Motor drive board )\n\r" );
00266     pc.printf( "\n\r" );
00267     while( pushsw_get() );
00268     wait(0.5);
00269     while( !pushsw_get() );
00270     while( 1 ){
00271         ImageData_Serial_Out2( ImageBinary, 20 );
00272     }
00273 }
00274 
00275 //******************************************************************//
00276 // @brief       Interrupt callback function
00277 // @param[in]   int_type    : VDC5 interrupt type
00278 // @retval      None
00279 //*******************************************************************/
00280 static void IntCallbackFunc_Vfield(DisplayBase::int_type_t int_type)
00281 {
00282     if (vfield_count > 0) {
00283         vfield_count--;
00284     }
00285     /* top or bottom (Change) */
00286     if      ( vfield_count2 == 0 )  vfield_count2 = 1;
00287     else if ( vfield_count2 == 1 )  vfield_count2 = 0;
00288 }
00289  
00290 //******************************************************************//
00291 // @brief       Wait for the specified number of times Vsync occurs
00292 // @param[in]   wait_count          : Wait count
00293 // @retval      None
00294 //*******************************************************************/
00295 static void WaitVfield(const int32_t wait_count)
00296 {
00297     vfield_count = wait_count;
00298     while (vfield_count > 0) {
00299         /* Do nothing */
00300     }
00301 }
00302  
00303 //******************************************************************//
00304 // @brief       Interrupt callback function for Vsync interruption
00305 // @param[in]   int_type    : VDC5 interrupt type
00306 // @retval      None
00307 //*******************************************************************/
00308 static void IntCallbackFunc_Vsync(DisplayBase::int_type_t int_type)
00309 {
00310     if (vsync_count > 0) {
00311         vsync_count--;
00312     }
00313 }
00314  
00315 //******************************************************************//
00316 // @brief       Wait for the specified number of times Vsync occurs
00317 // @param[in]   wait_count          : Wait count
00318 // @retval      None
00319 //*******************************************************************/
00320 static void WaitVsync(const int32_t wait_count)
00321 {
00322     vsync_count = wait_count;
00323     while (vsync_count > 0) {
00324         /* Do nothing */
00325     }
00326 }
00327 
00328 //******************************************************************//
00329 // Interrupt function( intTimer )
00330 //*******************************************************************/
00331 void intTimer( void )
00332 {
00333     static int  counter = 0;
00334  
00335     cnt0++;
00336     cnt1++;
00337  
00338     /* field check */
00339     if( vfield_count2 == vfield_count2_buff ) {
00340         vfield_count2_buff = vfield_count2;
00341     }
00342     /* Top field */
00343     if( !vfield_count2 ) {
00344         led_m_user( 1 );
00345         switch( counter++ ) {
00346         case 0:
00347             Image_Extraction( write_buff_addr, ImageData_A, vfield_count2 );
00348             break;
00349         case 1:
00350             Image_Extraction( write_buff_addr, ImageData_A, vfield_count2 );
00351             break;
00352         case 2:
00353             Image_Reduction( ImageData_A, 160, ImageComp_A, 8 );
00354             break;
00355         case 3:
00356             Image_Reduction( ImageData_A, 160, ImageComp_A, 8 );
00357             break;
00358         case 4:
00359             Binarization_process( ImageComp_A, ImageBinary, 20*15, 128 );
00360             break;
00361         case 5:
00362             /* Trace by image processing */
00363             break;
00364         case 6:
00365             //MarkCheck_Triangle
00366             MarkDetect_process_T();
00367             break;
00368         case 15:
00369             counter = 0;
00370             break;
00371         default:
00372             break;
00373         }
00374     }
00375     /* bottom field */
00376     else {
00377         led_m_user( 0 );
00378         switch( counter++ ) {
00379         case 0:
00380             Image_Extraction( write_buff_addr, ImageData_A, vfield_count2 );
00381             break;
00382         case 1:
00383             Image_Extraction( write_buff_addr, ImageData_A, vfield_count2 );
00384             break;
00385         case 2:
00386             Image_Reduction( ImageData_A, 160, ImageComp_A, 8 );
00387             break;
00388         case 3:
00389             Image_Reduction( ImageData_A, 160, ImageComp_A, 8 );
00390             break;
00391         case 4:
00392             Binarization_process( ImageComp_A, ImageBinary, 20*15, 128 );
00393             break;
00394         case 5:
00395             /* Trace by image processing */
00396             break;
00397         case 6:
00398             //MarkCheck_Triangle
00399             MarkDetect_process_T();
00400             break;
00401         case 15:
00402             counter = 0;
00403             break;
00404         default:
00405             break;
00406         }
00407     }
00408 
00409     /* LED processing */
00410     led_m_process();
00411 }
00412 
00413 //******************************************************************//
00414 // functions ( on GR-PEACH board )
00415 //*******************************************************************/
00416 //led_rgb Function
00417 //------------------------------------------------------------------//
00418 void led_rgb(int led)
00419 {
00420     LED_R = led & 0x1;
00421     LED_G = (led >> 1 ) & 0x1;
00422     LED_B = (led >> 2 ) & 0x1;
00423 }
00424  
00425 //user_button_get Function
00426 //------------------------------------------------------------------//
00427 unsigned int user_button_get( void )
00428 {
00429     return (~user_botton) & 0x1;        /* Read ports with switches */
00430 }
00431 
00432 //led_m_user Function
00433 //------------------------------------------------------------------//
00434 void led_m_user( int led )
00435 {
00436     USER_LED = led & 0x01;
00437 }
00438 
00439 //Lled_m_set Function
00440 //------------------------------------------------------------------//
00441 void led_m_set( int set )
00442 {
00443     led_set = set;
00444 }
00445 
00446 //led_m_process Function for only interrupt
00447 //------------------------------------------------------------------//
00448 void led_m_process( void )
00449 {
00450     static unsigned long    led_timer;
00451 
00452     led_timer++;
00453 
00454     /* Display */
00455     if( led_timer < led_data[led_set][1] ) led_rgb( led_data[led_set][0] );
00456     else if( led_timer < ( led_data[led_set][1] + led_data[led_set][2] ) ) led_rgb( LED_OFF );
00457     else led_timer = 0;
00458 }
00459 
00460 //******************************************************************//
00461 // functions ( on Motor drive board )
00462 //*******************************************************************/
00463 //led_out Function
00464 //------------------------------------------------------------------//
00465 void led_out(int led)
00466 {
00467     led = ~led;
00468     LED_3 = led & 0x1;
00469     LED_2 = ( led >> 1 ) & 0x1;
00470 }
00471  
00472 //pushsw_get Function
00473 //------------------------------------------------------------------//
00474 unsigned int pushsw_get( void )
00475 {
00476     return (~push_sw) & 0x1;            /* Read ports with switches */
00477 }
00478 
00479 //******************************************************************//
00480 // Image process functions
00481 //*******************************************************************/
00482 //Image Data YCbCr -> Y(320*240pix) -> Y(160*120)
00483 //frame 0 : Top field
00484 //frame 1 : Bottom field
00485 //------------------------------------------------------------------//
00486 void Image_Extraction( unsigned char *buff_addr, unsigned char *Data_Y, int frame )
00487 {
00488     static int  Xp, Yp, inc, Data_Y_buff;
00489     static int  counter = 0;
00490 
00491     // Distributed processing
00492     switch( counter++ ) {
00493     case 0:
00494         for( Yp = frame, inc = 0; Yp < 120; Yp+=2 ){
00495             for( Xp = 0; Xp < 640; Xp+=4, inc++ ){
00496                 Data_Y_buff   = (int)buff_addr[(Xp+0)+(640*Yp)];
00497                 Data_Y_buff  += (int)buff_addr[(Xp+2)+(640*Yp)];
00498                 Data_Y[inc]   = Data_Y_buff >> 1;
00499             }
00500         }
00501         break;
00502     case 1:
00503         for(     /* None */     ; Yp < 240; Yp+=2 ){
00504             for( Xp = 0; Xp < 640; Xp+=4, inc++ ){
00505                 Data_Y_buff   = (int)buff_addr[(Xp+0)+(640*Yp)];
00506                 Data_Y_buff  += (int)buff_addr[(Xp+2)+(640*Yp)];
00507                 Data_Y[inc]   = Data_Y_buff >> 1;
00508             }
00509         }
00510         counter = 0;
00511         break;
00512     default:
00513         break;
00514     }    
00515 }
00516  
00517 //Image_Reduction Function ( Averaging processing )
00518 //------------------------------------------------------------------//
00519 void Image_Reduction( unsigned char *Data_Y, int Data_W , unsigned char *Comp_Y, int Comp_M )
00520 {
00521     int         Data_H, Pixel_T, Pixel_D;
00522     int         x, y;
00523     static int  Xp, Yp, inc;
00524     static int  counter = 0;
00525  
00526     Data_H  = (Data_W / (double)4) * 3;
00527     Pixel_D = Comp_M * Comp_M;
00528 
00529     switch( counter++ ) {
00530     case 0:
00531         for( Yp = 0, inc = 0; Yp < ( Data_H / 2); Yp+=Comp_M ){
00532             for( Xp = 0; Xp < Data_W; Xp+=Comp_M, inc++ ){
00533                 Pixel_T = 0;            
00534                 for( y = 0; y < Comp_M; y++ ){
00535                     for( x = 0; x < Comp_M; x++ ){
00536                         Pixel_T += Data_Y[( Xp + x ) + (( Yp + y ) * Data_W )];
00537                     }
00538                 }
00539                 Comp_Y[inc] = Pixel_T / Pixel_D;
00540             }
00541         }
00542         break;
00543     case 1:
00544         for(   /* None */   ; Yp < Data_H       ; Yp+=Comp_M ){
00545             for( Xp = 0; Xp < Data_W; Xp+=Comp_M, inc++ ){
00546                 Pixel_T = 0;            
00547                 for( y = 0; y < Comp_M; y++ ){
00548                     for( x = 0; x < Comp_M; x++ ){
00549                         Pixel_T += Data_Y[( Xp + x ) + (( Yp + y ) * Data_W )];
00550                     }
00551                 }
00552                 Comp_Y[inc] = Pixel_T / Pixel_D;
00553             }
00554         }
00555         counter = 0;
00556         break;
00557     default:
00558         break;
00559     }
00560 }
00561  
00562 // Binarization_process Function
00563 //------------------------------------------------------------------//
00564 void Binarization_process( unsigned char *Comp_Y, unsigned char *Binary, long items, int threshold )
00565 {
00566     int     i;
00567  
00568     for( i = 0; i < items; i++ ) {
00569         if( Comp_Y[i] >= threshold )   Binary[i] = 1;
00570         else                           Binary[i] = 0;
00571     }
00572 }
00573 
00574 //******************************************************************//
00575 // Mark detect functions
00576 //*******************************************************************/
00577 // Extract_Image
00578 //------------------------------------------------------------------//
00579 void Image_part_Extraction( unsigned char *Binary, int Width, int Xpix, int Ypix, unsigned char *Data_B, int x_size, int y_size )
00580 {
00581     int     x, y;
00582     for( y = 0; y < y_size; y++ ) {
00583         for( x = 0; x < x_size; x++ ) {
00584             Data_B[ x + ( y * x_size ) ] = Binary[ (Xpix + x) + ( (Ypix + y) * Width ) ];
00585        }
00586     }
00587 }
00588 
00589 // Standard deviation
00590 //------------------------------------------------------------------//
00591 double Standard_Deviation( unsigned char *data, double *Devi, int items )
00592 {
00593     int         i;
00594     double      iRet_A, iRet_C, iRet_D;
00595 
00596     /* A 合計値　平均化 */
00597     iRet_A = 0;
00598     for( i = 0; i < items; i++ ) {
00599         iRet_A += data[i];
00600     }
00601     iRet_A /= items;
00602 
00603     /* B 偏差値 */
00604     for( i = 0; i < items; i++ ) {
00605         Devi[i] = data[i] - iRet_A;
00606     }
00607 
00608     /* C 分散 */
00609     iRet_C = 0;
00610     for( i = 0; i < items; i++ ) {
00611         iRet_C += ( Devi[i] * Devi[i] );
00612     }
00613     iRet_C /= items;
00614 
00615     /* D 標準偏差 */
00616     iRet_D = sqrt( iRet_C );
00617 
00618     return iRet_D;
00619 }
00620 
00621 // Covariance
00622 //------------------------------------------------------------------//
00623 double Covariance( double *Devi_A, double *Devi_B, int items )
00624 {
00625     int     i;
00626     double  iRet, iRet_buff;
00627 
00628     iRet = 0;
00629     for( i = 0; i < items; i++ ) {
00630         iRet_buff = Devi_A[i] * Devi_B[i];
00631         iRet     += iRet_buff;
00632     }
00633     iRet /= items;
00634 
00635     return iRet;
00636 }
00637 
00638 // Judgement_ImageMatching
00639 //------------------------------------------------------------------//
00640 int Judgement_ImageMatching( double covari, double SDevi_A, double SDevi_B )
00641 {
00642     int     iRet;
00643 
00644     iRet  = ( covari * 100 ) / ( SDevi_A * SDevi_B );
00645 
00646     return iRet;
00647 }
00648 
00649 // MarkDetect_process_T
00650 //------------------------------------------------------------------//
00651 void MarkDetect_process_T( void )
00652 {
00653     int  x, y;
00654 
00655     retJudgeIM_Max[0] = 0;
00656     for( y = 0; y <= 12; y++ ) {
00657         for( x = 0; x <= 15; x++ ) {
00658             Image_part_Extraction( ImageBinary, 20, x, y, NowImageBinary, 5, 3 );
00659             retDevi    = Standard_Deviation( NowImageBinary, NowDevi, 15 );
00660             retCovari  = Covariance( TempDevi_Triangle, NowDevi, 15 );
00661             retJudgeIM = 0;
00662             retJudgeIM = Judgement_ImageMatching( retCovari, retDevi_Triangle, retDevi );
00663             if( 100 >= retJudgeIM && retJudgeIM > retJudgeIM_Max[0] ) {
00664                 Xt = x;
00665                 Yt = y;
00666                 retJudgeIM_Max[0] = retJudgeIM;
00667             }
00668         }
00669     }
00670 }
00671 
00672 // MarkCheck Triangle detection
00673 // Return values: 0: no triangle mark, 1: Triangle mark
00674 //------------------------------------------------------------------//
00675 int MarkCheck_Triangle( int percentage )
00676 {
00677     int ret;
00678 
00679     ret = 0;
00680     if( retJudgeIM_Max[0] >= percentage ) {
00681         ret = 1;
00682     }
00683 
00684     return ret;
00685 }
00686 
00687 //******************************************************************//
00688 // Debug functions
00689 //*******************************************************************/
00690 //Image Data Output( for the Excel )
00691 //------------------------------------------------------------------//
00692 void ImageData_Serial_Out( unsigned char *Data_Y, int Width )
00693 {
00694     int     Xp, Yp, inc, Height;
00695  
00696     Height = (Width / (double)4) * 3;
00697     for( Yp = 0, inc = 0; Yp < Height; Yp++ ) {
00698         for( Xp = 0; Xp < Width; Xp++, inc++ ) {
00699             pc.printf( "%d,", Data_Y[ inc ] );
00700         }
00701         pc.printf("\n\r");
00702     }
00703 }
00704  
00705 //Image Data Output2( for TeraTerm )
00706 //------------------------------------------------------------------//
00707 void ImageData_Serial_Out2( unsigned char *Data_Y, int Width )
00708 {
00709     int     Xp, Yp, Height;
00710  
00711     Height = (Width / (double)4) * 3;
00712     for( Yp = 0; Yp < Height; Yp++ ) {
00713         for( Xp = 0; Xp < Width; Xp++ ) {
00714             pc.printf( "%d ", Data_Y[Xp + (Yp * Width)] );
00715         }
00716         pc.printf( "\n\r" );
00717     }
00718 
00719     //Add display
00720     pc.printf( "\n\r" );
00721     pc.printf( "T = %3d%% %01d X=%2d Y=%2d\n\r", retJudgeIM_Max[0], MarkCheck_Triangle( 90 ),  Xt, Yt );
00722     pc.printf( "\n\r" );
00723     Height += 3;
00724 
00725     pc.printf( "\033[%dA" , Height );
00726 }
00727 
00728 //------------------------------------------------------------------//
00729 // End of file
00730 //------------------------------------------------------------------//