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