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             break;
00424         }
00425         switch( (digital_sensor()&0x0f) ) {
00426             case 0x08:
00427                 handle( -20 );
00428                 motor( diff(100), 100 );
00429                 break;
00430             case 0x00:
00431             case 0x01:
00432             case 0x03:
00433                 handle( -22 );
00434                 motor( diff(100), 100 );
00435                 break;
00436             default:
00437                 break;
00438         }
00439         break;
00440 
00441     case 91:
00442         /* Stop */
00443         handle( 0 );
00444         motor( 0, 0 );
00445         break;
00446 
00447     default:
00448         break;
00449     }
00450     }
00451 }
00452 
00453 //******************************************************************//
00454 // Initialize functions
00455 //*******************************************************************/
00456 //Initialize MTU2 PWM functions
00457 //------------------------------------------------------------------//
00458 //MTU2_3, MTU2_4
00459 //Reset-Synchronized PWM mode
00460 //TIOC4A(P4_4) :Left-motor
00461 //TIOC4B(P4_5) :Right-motor
00462 //------------------------------------------------------------------//
00463 void init_MTU2_PWM_Motor( void )
00464 {
00465     /* Port setting for S/W I/O Contorol */
00466     /* alternative mode     */
00467  
00468     /* MTU2_4 (P4_4)(P4_5)  */
00469     GPIOPBDC4   = 0x0000;               /* Bidirection mode disabled*/
00470     GPIOPFCAE4 &= 0xffcf;               /* The alternative function of a pin */
00471     GPIOPFCE4  |= 0x0030;               /* The alternative function of a pin */
00472     GPIOPFC4   &= 0xffcf;               /* The alternative function of a pin */
00473                                         /* 2nd altemative function/output   */
00474     GPIOP4     &= 0xffcf;               /*                          */
00475     GPIOPM4    &= 0xffcf;               /* p4_4,P4_5:output         */
00476     GPIOPMC4   |= 0x0030;               /* P4_4,P4_5:double         */
00477  
00478     /* Mosule stop 33(MTU2) canceling */
00479     CPGSTBCR3  &= 0xf7;
00480  
00481     /* MTU2_3 and MTU2_4 (Motor PWM) */
00482     MTU2TCR_3   = 0x20;                 /* TCNT Clear(TGRA), P0φ/1  */
00483     MTU2TOCR1   = 0x04;                 /*                          */
00484     MTU2TOCR2   = 0x40;                 /* N L>H  P H>L             */
00485     MTU2TMDR_3  = 0x38;                 /* Buff:ON Reset-Synchronized PWM mode */
00486     MTU2TMDR_4  = 0x30;                 /* Buff:ON                  */
00487     MTU2TOER    = 0xc6;                 /* TIOC3B,4A,4B enabled output */
00488     MTU2TCNT_3  = MTU2TCNT_4 = 0;       /* TCNT3,TCNT4 Set 0        */
00489     MTU2TGRA_3  = MTU2TGRC_3 = MOTOR_PWM_CYCLE;
00490                                         /* PWM-Cycle(1ms)           */
00491     MTU2TGRA_4  = MTU2TGRC_4 = 0;       /* Left-motor(P4_4)         */
00492     MTU2TGRB_4  = MTU2TGRD_4 = 0;       /* Right-motor(P4_5)        */
00493     MTU2TSTR   |= 0x40;                 /* TCNT_4 Start             */
00494 }
00495  
00496 //Initialize MTU2 PWM functions
00497 //------------------------------------------------------------------//
00498 //MTU2_0
00499 //PWM mode 1
00500 //TIOC0A(P4_0) :Servo-motor
00501 //------------------------------------------------------------------//
00502 void init_MTU2_PWM_Servo( void )
00503 {
00504     /* Port setting for S/W I/O Contorol */
00505     /* alternative mode     */
00506  
00507     /* MTU2_0 (P4_0)        */
00508     GPIOPBDC4   = 0x0000;               /* Bidirection mode disabled*/
00509     GPIOPFCAE4 &= 0xfffe;               /* The alternative function of a pin */
00510     GPIOPFCE4  &= 0xfffe;               /* The alternative function of a pin */
00511     GPIOPFC4   |= 0x0001;               /* The alternative function of a pin */
00512                                         /* 2nd alternative function/output   */
00513     GPIOP4     &= 0xfffe;               /*                          */
00514     GPIOPM4    &= 0xfffe;               /* p4_0:output              */
00515     GPIOPMC4   |= 0x0001;               /* P4_0:double              */
00516  
00517     /* Mosule stop 33(MTU2) canceling */
00518     CPGSTBCR3  &= 0xf7;
00519  
00520     /* MTU2_0 (Motor PWM) */
00521     MTU2TCR_0   = 0x22;                 /* TCNT Clear(TGRA), P0φ/16 */
00522     MTU2TIORH_0 = 0x52;                 /* TGRA L>H, TGRB H>L       */
00523     MTU2TMDR_0  = 0x32;                 /* TGRC and TGRD = Buff-mode*/
00524                                         /* PWM-mode1                */
00525     MTU2TCNT_0  = 0;                    /* TCNT0 Set 0              */
00526     MTU2TGRA_0  = MTU2TGRC_0 = SERVO_PWM_CYCLE;
00527                                         /* PWM-Cycle(16ms)          */
00528     MTU2TGRB_0  = MTU2TGRD_0 = 0;       /* Servo-motor(P4_0)        */
00529     MTU2TSTR   |= 0x01;                 /* TCNT_0 Start             */
00530 }
00531 
00532 //******************************************************************//
00533 // @brief       Interrupt callback function
00534 // @param[in]   int_type    : VDC5 interrupt type
00535 // @retval      None
00536 //*******************************************************************/
00537 static void IntCallbackFunc_Vfield(DisplayBase::int_type_t int_type)
00538 {
00539     if (vfield_count > 0) {
00540         vfield_count--;
00541     }
00542     /* top or bottom (Change) */
00543     if      ( vfield_count2 == 0 )  vfield_count2 = 1;
00544     else if ( vfield_count2 == 1 )  vfield_count2 = 0;
00545 }
00546  
00547 //******************************************************************//
00548 // @brief       Wait for the specified number of times Vsync occurs
00549 // @param[in]   wait_count          : Wait count
00550 // @retval      None
00551 //*******************************************************************/
00552 static void WaitVfield(const int32_t wait_count)
00553 {
00554     vfield_count = wait_count;
00555     while (vfield_count > 0) {
00556         /* Do nothing */
00557     }
00558 }
00559  
00560 //******************************************************************//
00561 // @brief       Interrupt callback function for Vsync interruption
00562 // @param[in]   int_type    : VDC5 interrupt type
00563 // @retval      None
00564 //*******************************************************************/
00565 static void IntCallbackFunc_Vsync(DisplayBase::int_type_t int_type)
00566 {
00567     if (vsync_count > 0) {
00568         vsync_count--;
00569     }
00570 }
00571  
00572 //******************************************************************//
00573 // @brief       Wait for the specified number of times Vsync occurs
00574 // @param[in]   wait_count          : Wait count
00575 // @retval      None
00576 //*******************************************************************/
00577 static void WaitVsync(const int32_t wait_count)
00578 {
00579     vsync_count = wait_count;
00580     while (vsync_count > 0) {
00581         /* Do nothing */
00582     }
00583 }
00584 
00585 //******************************************************************//
00586 // Interrupt function( intTimer )
00587 //*******************************************************************/
00588 void intTimer( void )
00589 {
00590     static int  counter = 0;
00591  
00592     cnt0++;
00593     cnt1++;
00594  
00595     /* field check */
00596     if( vfield_count2 != vfield_count2_buff ) {
00597         vfield_count2_buff = vfield_count2;
00598         counter = 0;
00599     }
00600     /* Top field / bottom field */
00601     led_m_user( vfield_count2 );
00602     switch( counter++ ) {
00603     case 0:
00604         Image_Extraction( write_buff_addr, ImageData_A, vfield_count2 );
00605         break;
00606     case 1:
00607         Image_Extraction( write_buff_addr, ImageData_A, vfield_count2 );
00608         break;
00609     case 2:
00610         Image_Reduction( ImageData_A, 160, ImageComp_A, 8 );
00611         break;
00612     case 3:
00613         Image_Reduction( ImageData_A, 160, ImageComp_A, 8 );
00614         break;
00615     case 4:
00616         Binarization_process( ImageComp_A, ImageBinary, 20*15, 128 );
00617         break;
00618     case 5:
00619         /* Trace by image processing */
00620         digital_sensor_process( ImageBinary );
00621         break;
00622     case 6:
00623         //MarkCheck_Triangle
00624         MarkDetect_process_T();
00625         break;
00626     default:
00627         break;
00628     }
00629 
00630     /* LED processing */
00631     led_m_process();
00632 }
00633 
00634 //******************************************************************//
00635 // functions ( on GR-PEACH board )
00636 //*******************************************************************/
00637 //led_rgb Function
00638 //------------------------------------------------------------------//
00639 void led_rgb(int led)
00640 {
00641     LED_R = led & 0x1;
00642     LED_G = (led >> 1 ) & 0x1;
00643     LED_B = (led >> 2 ) & 0x1;
00644 }
00645  
00646 //user_button_get Function
00647 //------------------------------------------------------------------//
00648 unsigned int user_button_get( void )
00649 {
00650     return (~user_botton) & 0x1;        /* Read ports with switches */
00651 }
00652 
00653 //led_m_user Function
00654 //------------------------------------------------------------------//
00655 void led_m_user( int led )
00656 {
00657     USER_LED = led & 0x01;
00658 }
00659 
00660 //Lled_m_set Function
00661 //------------------------------------------------------------------//
00662 void led_m_set( int set )
00663 {
00664     led_set = set;
00665 }
00666 
00667 //led_m_process Function for only interrupt
00668 //------------------------------------------------------------------//
00669 void led_m_process( void )
00670 {
00671     static unsigned long    led_timer;
00672 
00673     led_timer++;
00674 
00675     /* Display */
00676     if( led_timer < led_data[led_set][1] ) led_rgb( led_data[led_set][0] );
00677     else if( led_timer < ( led_data[led_set][1] + led_data[led_set][2] ) ) led_rgb( LED_OFF );
00678     else led_timer = 0;
00679 }
00680 
00681 //******************************************************************//
00682 // functions ( on Motor drive board )
00683 //*******************************************************************/
00684 //led_out Function
00685 //------------------------------------------------------------------//
00686 void led_out(int led)
00687 {
00688     led = ~led;
00689     LED_3 = led & 0x1;
00690     LED_2 = ( led >> 1 ) & 0x1;
00691 }
00692  
00693 //pushsw_get Function
00694 //------------------------------------------------------------------//
00695 unsigned int pushsw_get( void )
00696 {
00697     return (~push_sw) & 0x1;            /* Read ports with switches */
00698 }
00699  
00700 //motor speed control(PWM)
00701 //Arguments: motor:-100 to 100
00702 //Here, 0 is stop, 100 is forward, -100 is reverse
00703 //------------------------------------------------------------------//
00704 void motor( int accele_l, int accele_r )
00705 {
00706     accele_l = ( accele_l * MAX_SPEED ) / 100;
00707     accele_r = ( accele_r * MAX_SPEED ) / 100;
00708  
00709     /* Left Motor Control */
00710     if( accele_l >= 0 ) {
00711         /* forward */
00712         Left_motor_signal = 0;
00713         MTU2TGRC_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * accele_l / 100;
00714     } else {
00715         /* reverse */
00716         Left_motor_signal = 1;
00717         MTU2TGRC_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * ( -accele_l ) / 100;
00718     }
00719  
00720     /* Right Motor Control */
00721     if( accele_r >= 0 ) {
00722         /* forward */
00723         Right_motor_signal = 0;
00724         MTU2TGRD_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * accele_r / 100;
00725     } else {
00726         /* reverse */
00727         Right_motor_signal = 1;
00728         MTU2TGRD_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * ( -accele_r ) / 100;
00729     }
00730 }
00731  
00732 //handle Function
00733 //------------------------------------------------------------------//
00734 void handle( int angle )
00735 {
00736     handle_buff = angle;
00737     /* When the servo move from left to right in reverse, replace "-" with "+" */
00738     MTU2TGRD_0 = SERVO_CENTER - angle * HANDLE_STEP;
00739 }
00740  
00741 //diff Function
00742 //------------------------------------------------------------------//
00743 int diff( int pwm )
00744 {
00745     int i, ret;
00746  
00747     i  = handle_buff;
00748     if( i <  0 ) i = -i;
00749     if( i > 45 ) i = 45;
00750     ret = revolution_difference[i] * pwm / 100;
00751  
00752     return ret;
00753 }
00754 
00755 //******************************************************************//
00756 // Image process functions
00757 //*******************************************************************/
00758 //Image Data YCbCr -> Y(320*240pix) -> Y(160*120)
00759 //frame 0 : Top field
00760 //frame 1 : Bottom field
00761 //------------------------------------------------------------------//
00762 void Image_Extraction( unsigned char *buff_addr, unsigned char *Data_Y, int frame )
00763 {
00764     static int  Xp, Yp, inc, Data_Y_buff;
00765     static int  counter = 0;
00766 
00767     // Distributed processing
00768     switch( counter++ ) {
00769     case 0:
00770         for( Yp = frame, inc = 0; Yp < 120; Yp+=2 ){
00771             for( Xp = 0; Xp < 640; Xp+=4, inc++ ){
00772                 Data_Y_buff   = (int)buff_addr[(Xp+0)+(640*Yp)];
00773                 Data_Y_buff  += (int)buff_addr[(Xp+2)+(640*Yp)];
00774                 Data_Y[inc]   = Data_Y_buff >> 1;
00775             }
00776         }
00777         break;
00778     case 1:
00779         for(     /* None */     ; Yp < 240; Yp+=2 ){
00780             for( Xp = 0; Xp < 640; Xp+=4, inc++ ){
00781                 Data_Y_buff   = (int)buff_addr[(Xp+0)+(640*Yp)];
00782                 Data_Y_buff  += (int)buff_addr[(Xp+2)+(640*Yp)];
00783                 Data_Y[inc]   = Data_Y_buff >> 1;
00784             }
00785         }
00786         counter = 0;
00787         break;
00788     default:
00789         break;
00790     }    
00791 }
00792  
00793 //Image_Reduction Function ( Averaging processing )
00794 //------------------------------------------------------------------//
00795 void Image_Reduction( unsigned char *Data_Y, int Data_W , unsigned char *Comp_Y, int Comp_M )
00796 {
00797     int         Data_H, Pixel_T, Pixel_D;
00798     int         x, y;
00799     static int  Xp, Yp, inc;
00800     static int  counter = 0;
00801  
00802     Data_H  = (Data_W / (double)4) * 3;
00803     Pixel_D = Comp_M * Comp_M;
00804 
00805     switch( counter++ ) {
00806     case 0:
00807         for( Yp = 0, inc = 0; Yp < ( Data_H / 2); Yp+=Comp_M ){
00808             for( Xp = 0; Xp < Data_W; Xp+=Comp_M, inc++ ){
00809                 Pixel_T = 0;            
00810                 for( y = 0; y < Comp_M; y++ ){
00811                     for( x = 0; x < Comp_M; x++ ){
00812                         Pixel_T += Data_Y[( Xp + x ) + (( Yp + y ) * Data_W )];
00813                     }
00814                 }
00815                 Comp_Y[inc] = Pixel_T / Pixel_D;
00816             }
00817         }
00818         break;
00819     case 1:
00820         for(   /* None */   ; Yp < Data_H       ; Yp+=Comp_M ){
00821             for( Xp = 0; Xp < Data_W; Xp+=Comp_M, inc++ ){
00822                 Pixel_T = 0;            
00823                 for( y = 0; y < Comp_M; y++ ){
00824                     for( x = 0; x < Comp_M; x++ ){
00825                         Pixel_T += Data_Y[( Xp + x ) + (( Yp + y ) * Data_W )];
00826                     }
00827                 }
00828                 Comp_Y[inc] = Pixel_T / Pixel_D;
00829             }
00830         }
00831         counter = 0;
00832         break;
00833     default:
00834         break;
00835     }
00836 }
00837  
00838 // Binarization_process Function
00839 //------------------------------------------------------------------//
00840 void Binarization_process( unsigned char *Comp_Y, unsigned char *Binary, long items, int threshold )
00841 {
00842     int     i;
00843  
00844     for( i = 0; i < items; i++ ) {
00845         if( Comp_Y[i] >= threshold )   Binary[i] = 1;
00846         else                           Binary[i] = 0;
00847     }
00848 }
00849 
00850 //******************************************************************//
00851 // digital sensor functions
00852 //*******************************************************************/
00853 //CenterLine_Corrective Function ( image size 20*15pix )
00854 //------------------------------------------------------------------//
00855 int CenterLine_Corrective( unsigned char *Binary )
00856 {
00857     #define L       0
00858     #define R       1
00859  
00860     int iRet, offset_X, offset_Y;
00861     int Xpix, X;
00862     int Ypix;
00863     int Pixel_diff[2];
00864     int Error_cnt;
00865     int value;
00866  
00867     /* Center of image */
00868     offset_X = 6;
00869     offset_Y = 12;
00870  
00871     /* corrective of center line */
00872     for( Ypix = 0, Error_cnt = 0; Ypix < (offset_Y - 4); Ypix++ ) {
00873         for( value = 0; value < 2; value++ ) {
00874             for( Xpix = offset_X; Xpix < (offset_X + 8); Xpix++ ) {
00875                 /* Lift side */
00876                 Pixel_diff[L] = 0;
00877                 if( Binary[ ( ( offset_Y - Ypix ) * 20 ) + Xpix ] >= 1 ) {
00878                     for( X = Xpix; X > (Xpix - 4); X-- ) {
00879                         if( Binary[ ( ( offset_Y - Ypix ) * 20 ) + X ] >= 1 ) {
00880                             Pixel_diff[L]++;
00881                         } else {
00882                             break;
00883                         }
00884                     }
00885                 } else {
00886                     Pixel_diff[L] = -1;
00887                 }
00888                 /* Right side */
00889                 Pixel_diff[R] = 0;
00890                 if( Binary[ ( ( offset_Y - Ypix ) * 20 ) + (Xpix + 1) ] >= 1 ) {
00891                     for( X = (Xpix + 1); X < ((Xpix + 1) + 4); X++ ) {
00892                         if( Binary[ ( ( offset_Y - Ypix ) * 20 ) + X ] >= 1 ) {
00893                             Pixel_diff[R]++;
00894                         } else {
00895                             break;
00896                         }
00897                     }
00898                 } else {
00899                     Pixel_diff[R] = 1;
00900                 }
00901                 /* check */
00902                 iRet = Pixel_diff[L] - Pixel_diff[R];
00903                 if( value >= iRet && iRet >= -value ) {
00904                     break;
00905                 }
00906             }
00907             if( value >= iRet && iRet >= -value ) {
00908                 /* X coordinate */
00909                 Sensor_X[Ypix][2] = Xpix;
00910                 Sensor_X[Ypix][3] = Xpix + 1;
00911                 break;
00912             } else {
00913                 Sensor_X[Ypix][2] = Sensor_X[Ypix][3] = -1;
00914                 Error_cnt++;
00915             }
00916         }
00917         /* Left side sensor */
00918         Sensor_X[Ypix][1] = Sensor_X[Ypix][2] - ( Pixel_diff[L] );
00919         Sensor_X[Ypix][0] = Sensor_X[Ypix][1] - ( Pixel_diff[L] + 1 );//( Sensor_X[Ypix][2] - Sensor_X[Ypix][1] );
00920         /* Right side sensor */
00921         Sensor_X[Ypix][4] = Sensor_X[Ypix][3] + ( Pixel_diff[R] );
00922         Sensor_X[Ypix][5] = Sensor_X[Ypix][4] + ( Pixel_diff[R] + 1 );//( Sensor_X[Ypix][4] - Sensor_X[Ypix][3] );
00923     }
00924     return Error_cnt;
00925 }
00926  
00927 //digital_sensor_process Function
00928 //------------------------------------------------------------------//
00929 void digital_sensor_process( unsigned char *Binary )
00930 {
00931     int             Ypix;   // !!!! not initilazed !!!!
00932     int             offset_Y;
00933     unsigned char   sensor, data;
00934  
00935     offset_Y    = 12;
00936     sensor      = 0;
00937  
00938     data  = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][2] ] & 0x01;
00939     data |= Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][3] ] & 0x01;
00940     sensor |= (data << 4) & 0x10;
00941     data  = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][0] ] & 0x01;
00942     sensor |= (data << 3) & 0x08;
00943     data  = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][1] ] & 0x01;
00944     sensor |= (data << 2) & 0x04;
00945     data  = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][4] ] & 0x01;
00946     sensor |= (data << 1) & 0x02;
00947     data  = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][5] ] & 0x01;
00948     sensor |= (data << 0) & 0x01;
00949     sensor &= 0x1f;
00950     sensor_value = sensor;
00951 }
00952  
00953 //digital_sensor Function
00954 //------------------------------------------------------------------//
00955 unsigned char digital_sensor( void )
00956 {
00957     return sensor_value;
00958 }
00959 
00960 //******************************************************************//
00961 // Mark detect functions
00962 //*******************************************************************/
00963 // Extract_Image
00964 //------------------------------------------------------------------//
00965 void Image_part_Extraction( unsigned char *Binary, int Width, int Xpix, int Ypix, unsigned char *Data_B, int x_size, int y_size )
00966 {
00967     int     x, y;
00968     for( y = 0; y < y_size; y++ ) {
00969         for( x = 0; x < x_size; x++ ) {
00970             Data_B[ x + ( y * x_size ) ] = Binary[ (Xpix + x) + ( (Ypix + y) * Width ) ];
00971        }
00972     }
00973 }
00974 
00975 // Standard deviation
00976 //------------------------------------------------------------------//
00977 double Standard_Deviation( unsigned char *data, double *Devi, int items )
00978 {
00979     int         i;
00980     double      iRet_A, iRet_C, iRet_D;
00981 
00982     /* A 合計値　平均化 */
00983     iRet_A = 0;
00984     for( i = 0; i < items; i++ ) {
00985         iRet_A += data[i];
00986     }
00987     iRet_A /= items;
00988 
00989     /* B 偏差値 */
00990     for( i = 0; i < items; i++ ) {
00991         Devi[i] = data[i] - iRet_A;
00992     }
00993 
00994     /* C 分散 */
00995     iRet_C = 0;
00996     for( i = 0; i < items; i++ ) {
00997         iRet_C += ( Devi[i] * Devi[i] );
00998     }
00999     iRet_C /= items;
01000 
01001     /* D 標準偏差 */
01002     iRet_D = sqrt( iRet_C );
01003 
01004     return iRet_D;
01005 }
01006 
01007 // Covariance
01008 //------------------------------------------------------------------//
01009 double Covariance( double *Devi_A, double *Devi_B, int items )
01010 {
01011     int     i;
01012     double  iRet, iRet_buff;
01013 
01014     iRet = 0;
01015     for( i = 0; i < items; i++ ) {
01016         iRet_buff = Devi_A[i] * Devi_B[i];
01017         iRet     += iRet_buff;
01018     }
01019     iRet /= items;
01020 
01021     return iRet;
01022 }
01023 
01024 // Judgement_ImageMatching
01025 //------------------------------------------------------------------//
01026 int Judgement_ImageMatching( double covari, double SDevi_A, double SDevi_B )
01027 {
01028     int     iRet;
01029 
01030     iRet  = ( covari * 100 ) / ( SDevi_A * SDevi_B );
01031 
01032     return iRet;
01033 }
01034 
01035 // MarkDetect_process_T
01036 //------------------------------------------------------------------//
01037 void MarkDetect_process_T( void )
01038 {
01039     int  x, y;
01040 
01041     retJudgeIM_Max[0] = 0;
01042     for( y = 0; y <= 12; y++ ) {
01043         for( x = 0; x <= 15; x++ ) {
01044             Image_part_Extraction( ImageBinary, 20, x, y, NowImageBinary, 5, 3 );
01045             retDevi    = Standard_Deviation( NowImageBinary, NowDevi, 15 );
01046             retCovari  = Covariance( TempDevi_Triangle, NowDevi, 15 );
01047             retJudgeIM = 0;
01048             retJudgeIM = Judgement_ImageMatching( retCovari, retDevi_Triangle, retDevi );
01049             if( 100 >= retJudgeIM && retJudgeIM > retJudgeIM_Max[0] ) {
01050                 Xt = x;
01051                 Yt = y;
01052                 retJudgeIM_Max[0] = retJudgeIM;
01053             }
01054         }
01055     }
01056 }
01057 
01058 // MarkCheck Triangle detection
01059 // Return values: 0: no triangle mark, 1: Triangle mark
01060 //------------------------------------------------------------------//
01061 int MarkCheck_Triangle( int percentage )
01062 {
01063     int ret;
01064 
01065     ret = 0;
01066     if( retJudgeIM_Max[0] >= percentage ) {
01067         ret = 1;
01068     }
01069 
01070     return ret;
01071 }
01072 
01073 //******************************************************************//
01074 // Debug functions
01075 //*******************************************************************/
01076 //Image Data Output( for the Excel )
01077 //------------------------------------------------------------------//
01078 void ImageData_Serial_Out( unsigned char *Data_Y, int Width )
01079 {
01080     int     Xp, Yp, inc, Height;
01081  
01082     Height = (Width / (double)4) * 3;
01083     for( Yp = 0, inc = 0; Yp < Height; Yp++ ) {
01084         for( Xp = 0; Xp < Width; Xp++, inc++ ) {
01085             pc.printf( "%d,", Data_Y[ inc ] );
01086         }
01087         pc.printf("\n\r");
01088     }
01089 }
01090  
01091 //Image Data Output2( for TeraTerm )
01092 //------------------------------------------------------------------//
01093 void ImageData_Serial_Out2( unsigned char *Data_Y, int Width )
01094 {
01095     int     Xp, Yp, Height;
01096  
01097     Height = (Width / (double)4) * 3;
01098     for( Yp = 0; Yp < Height; Yp++ ) {
01099         for( Xp = 0; Xp < Width; Xp++ ) {
01100             pc.printf( "%d ", Data_Y[Xp + (Yp * Width)] );
01101         }
01102         pc.printf( "\n\r" );
01103     }
01104 
01105     //Add display
01106     pc.printf( "\n\r" );
01107     pc.printf( "sensor_inp = 0x%02x\n\r", digital_sensor() );
01108     pc.printf( "T = %3d%% %01d X=%2d Y=%2d\n\r", retJudgeIM_Max[0], MarkCheck_Triangle( 90 ),  Xt, Yt );
01109     pc.printf( "\n\r" );
01110     Height += 4;
01111 
01112     pc.printf( "\033[%dA" , Height );
01113 }
01114 
01115 //------------------------------------------------------------------//
01116 // End of file
01117 //------------------------------------------------------------------//