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