Revision 0:55e74a8600c6, committed 2016-09-02

Comitter:

TetsuyaKonno

Date:

Fri Sep 02 07:34:13 2016 +0000

Commit message:

First program

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GR-PEACH_video.lib	Fri Sep 02 07:34:13 2016 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/teams/Renesas/code/GR-PEACH_video/#aeefe5171463

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Sep 02 07:34:13 2016 +0000
@@ -0,0 +1,1290 @@
+//------------------------------------------------------------------//
+//Supported MCU:   RZ/A1H
+//File Contents:   Trace and Mark detection Program by Image Processing
+//                                 (GR-PEACH version on the Micon Car)
+//Version number:  Ver.1.00
+//Date:            2016.07.15
+//Copyright:       Renesas Electronics Corporation
+//------------------------------------------------------------------//
+ 
+//This program supports the following boards:
+//* GR-PEACH(E version)
+//* Motor drive board Ver.5
+//* Camera module (SC-310)
+ 
+//Include
+//------------------------------------------------------------------//
+#include "mbed.h"
+#include "math.h"
+#include "iodefine.h"
+#include "DisplayBace.h"
+//Define
+//------------------------------------------------------------------//
+//Motor PWM cycle
+#define     MOTOR_PWM_CYCLE     33332   /* Motor PWM period         */
+                                        /* 1ms    P0φ/1  = 0.03us   */
+//Motor speed
+#define     MAX_SPEED           45      /* motor()  set: 0 to 100   */
+ 
+//Servo PWM cycle
+#define     SERVO_PWM_CYCLE     33332   /* SERVO PWM period         */
+                                        /* 16ms   P0φ/16 = 0.48us   */
+#define     SERVO_CENTER        3124    /* 1.5ms / 0.48us - 1 = 3124*/
+#define     HANDLE_STEP         18      /* 1 degree value           */
+ 
+//LED Color on GR-PEACH
+#define     LED_OFF             0x00
+#define     LED_RED             0x01
+#define     LED_GREEN           0x02
+#define     LED_YELLOW          0x03
+#define     LED_BLUE            0x04
+#define     LED_PURPLE          0x05
+#define     LED_SKYBLUE         0x06
+#define     LED_WHITE           0x07
+ 
+//Status
+#define     RUN                 0x00
+#define     SENSOR              0x01
+#define     MARK_T              0x02
+#define     MARK_C              0x03
+#define     MARK_R              0x04
+#define     MARK_L              0x05
+#define     STOP                0x06
+#define     ERROR               0xff
+ 
+//Define(NTSC-Video)
+//------------------------------------------------------------------//
+#define VIDEO_INPUT_CH         (DisplayBase::VIDEO_INPUT_CHANNEL_0)
+#define VIDEO_INT_TYPE         (DisplayBase::INT_TYPE_S0_VFIELD)
+#define DATA_SIZE_PER_PIC      (2u)
+ 
+/*! Frame buffer stride: Frame buffer stride should be set to a multiple of 32 or 128
+    in accordance with the frame buffer burst transfer mode. */
+#define PIXEL_HW               (320u)  /* QVGA */
+#define PIXEL_VW               (240u)  /* QVGA */
+#define VIDEO_BUFFER_STRIDE    (((PIXEL_HW * DATA_SIZE_PER_PIC) + 31u) & ~31u)
+#define VIDEO_BUFFER_HEIGHT    (PIXEL_VW)
+ 
+//Constructor
+//------------------------------------------------------------------//
+Ticker      interrput;
+Serial      pc(USBTX, USBRX);
+DigitalOut  LED_R(P6_13);               /* LED1 on the GR-PEACH board */
+DigitalOut  LED_G(P6_14);               /* LED2 on the GR-PEACH board */
+DigitalOut  LED_B(P6_15);               /* LED3 on the GR-PEACH board */
+DigitalIn   user_botton(P6_0);          /* SW1 on the GR-PEACH board */
+ 
+DigitalOut  Left_motor_signal(P4_6);    /* Used by motor fanction   */
+DigitalOut  Right_motor_signal(P4_7);   /* Used by motor fanction   */
+DigitalIn   push_sw(P2_13);             /* SW1 on the Motor Drive board */
+DigitalOut  LED_3(P2_14);               /* LED3 on the Motor Drive board */
+DigitalOut  LED_2(P2_15);               /* LED2 on the Motor Drive board */
+ 
+//Prototype
+//------------------------------------------------------------------//
+//Peripheral functions
+void init_MTU2_PWM_Motor( void );       /* Initialize PWM functions */
+void init_MTU2_PWM_Servo( void );       /* Initialize PWM functions */
+void intTimer( void );                  /* Interrupt fanction       */
+ 
+//GR-peach board
+void led_rgb(int led);
+unsigned int user_button_get( void );
+ 
+//Motor drive board
+void led_out(int led);
+unsigned int pushsw_get( void );
+void motor( int accele_l, int accele_r );
+void handle( int angle );
+int diff( int pwm );
+ 
+//Interrupt function
+void led_status_process( void );        /* Function for only interrupt */
+void led_status_set( int set );
+ 
+//Prototype(NTSC-video)
+//------------------------------------------------------------------//
+static void IntCallbackFunc_Vfield(DisplayBase::int_type_t int_type);
+static void WaitVfield(const int32_t wait_count);
+static void IntCallbackFunc_Vsync(DisplayBase::int_type_t int_type);
+static void WaitVsync(const int32_t wait_count);
+ 
+//Prototype(Display Debug)
+//------------------------------------------------------------------//
+void ImageData_Serial_Out( unsigned char *Data_Y, int Width );
+void ImageData_Serial_Out2( unsigned char *Data_Y, int Width );
+void ImageData_Serial_Out3( void );
+ 
+//Prototype(Trace by Image Processing)
+//------------------------------------------------------------------//
+int CenterLine_Corrective( unsigned char *Binary );
+void change_framebuffer_process( void );                /* Function for only interrupt */
+void digital_sensor_process( unsigned char *Binary );   /* Function for only interrupt */
+unsigned char digital_sensor( void );
+ 
+//Prototype(Mark detection)
+//------------------------------------------------------------------//
+void Image_Extraction( unsigned char *Data_Y );
+void Image_part_Extraction( unsigned char *Binary, int Width, int Xpix, int Ypix, unsigned char *Data_B, int x_size, int y_size );
+void Image_Compression( unsigned char *Data_Y, int Data_W , unsigned char *Comp_Y, int Comp_W );
+void Image_Compression2( unsigned char *Data_Y, int Data_W , unsigned char *Comp_Y, int Comp_M );
+void Binarization_process( unsigned char *Comp_Y, unsigned char *Binary, long items );
+double Standard_Deviation( unsigned char *data, double *Devi, int items );
+double Covariance( double *Devi_A, double *Devi_B, int items );
+int Judgement_ImageMatching( double covari, double SDevi_A, double SDevi_B );
+int MarkCheck_Triangle( int percentage );
+ 
+//Globle
+//------------------------------------------------------------------//
+volatile unsigned long  cnt0;           /* Used by timer function   */
+volatile unsigned long  cnt1;           /* Used within main         */
+volatile int            pattern;        /* Pattern numbers          */
+volatile int            status_set;     /* Status                   */
+volatile int            handle_buff;
+ 
+const int revolution_difference[] = {
+    100, 98, 97, 95, 93, 
+    92, 90, 88, 87, 85, 
+    84, 82, 81, 79, 78, 
+    76, 75, 73, 72, 71, 
+    69, 68, 66, 65, 64, 
+    62, 61, 59, 58, 57, 
+    55, 54, 52, 51, 50, 
+    48, 47, 45, 44, 42, 
+    41, 39, 38, 36, 35, 
+    33 };
+ 
+/* Trace by image processing */
+volatile int            Sensor_X[8][6];
+volatile unsigned char  sensor_value;
+ 
+/* Mark detection */
+unsigned char   ImageData[320*240];
+unsigned char   ImageComp[160*120];
+unsigned char   ImageBinary[160*120];
+ 
+double          TempDevi_Triangle[15];
+unsigned char   TempBinary_Triangle[15] = {0,1,1,1,0,
+                                           0,0,1,0,0,
+                                           0,0,0,0,0};
+double          NowDevi[30];
+unsigned char   NowImageBinary[30];
+ 
+volatile double retDevi_Triangle;
+ 
+volatile double retDevi;
+volatile double retCovari;
+volatile int    retJudgeIM;
+volatile int    retJudgeIM_Max[1];
+ 
+int             Xt, Yt;
+ 
+//Globle(NTSC-video)
+//------------------------------------------------------------------//
+static uint8_t FrameBuffer_Video_A[VIDEO_BUFFER_STRIDE * VIDEO_BUFFER_HEIGHT]__attribute((section("NC_BSS"),aligned(16)));  //16 bytes aligned!;
+static uint8_t FrameBuffer_Video_B[VIDEO_BUFFER_STRIDE * VIDEO_BUFFER_HEIGHT]__attribute((section("NC_BSS"),aligned(16)));  //16 bytes aligned!;
+static volatile int32_t vsync_count;
+static volatile int32_t vfield_count;
+uint8_t * write_buff_addr = FrameBuffer_Video_A;
+uint8_t * save_buff_addr  = FrameBuffer_Video_B;
+ 
+//Main
+//------------------------------------------------------------------//
+int main( void )
+{
+    int Brake_cnt;       /* Local variables for only the pattern 91 */
+ 
+    /* NTSC-Video */
+    DisplayBase::graphics_error_t error;
+ 
+    /* Create DisplayBase object */
+    DisplayBase Display;
+ 
+    /* Graphics initialization process */
+    error = Display.Graphics_init(NULL);
+    if (error != DisplayBase::GRAPHICS_OK) {
+        printf("Line %d, error %d\n", __LINE__, error);
+        while (1);
+    }
+ 
+    error = Display.Graphics_Video_init( DisplayBase::INPUT_SEL_VDEC, NULL);
+    if( error != DisplayBase::GRAPHICS_OK ) {
+        while(1);
+    }
+ 
+    /* Interrupt callback function setting (Vsync signal input to scaler 0) */
+    error = Display.Graphics_Irq_Handler_Set(DisplayBase::INT_TYPE_S0_VI_VSYNC, 0, IntCallbackFunc_Vsync);
+    if (error != DisplayBase::GRAPHICS_OK) {
+        printf("Line %d, error %d\n", __LINE__, error);
+        while (1);
+    }
+ 
+    /* Video capture setting (progressive form fixed) */
+    error = Display.Video_Write_Setting(
+                VIDEO_INPUT_CH,
+                DisplayBase::COL_SYS_NTSC_358,
+                write_buff_addr,
+                VIDEO_BUFFER_STRIDE,
+                DisplayBase::VIDEO_FORMAT_YCBCR422,
+                DisplayBase::WR_RD_WRSWA_32_16BIT,
+                PIXEL_VW,
+                PIXEL_HW
+            );
+    if (error != DisplayBase::GRAPHICS_OK) {
+        printf("Line %d, error %d\n", __LINE__, error);
+        while (1);
+    }
+ 
+    /* Interrupt callback function setting (Field end signal for recording function in scaler 0) */
+    error = Display.Graphics_Irq_Handler_Set(VIDEO_INT_TYPE, 0, IntCallbackFunc_Vfield);
+    if (error != DisplayBase::GRAPHICS_OK) {
+        printf("Line %d, error %d\n", __LINE__, error);
+        while (1);
+    }
+ 
+    /* Video write process start */
+    error = Display.Video_Start (VIDEO_INPUT_CH);
+    if (error != DisplayBase::GRAPHICS_OK) {
+        printf("Line %d, error %d\n", __LINE__, error);
+        while (1);
+    }
+ 
+    /* Video write process stop */
+    error = Display.Video_Stop (VIDEO_INPUT_CH);
+    if (error != DisplayBase::GRAPHICS_OK) {
+        printf("Line %d, error %d\n", __LINE__, error);
+        while (1);
+    }
+ 
+    /* Video write process start */
+    error = Display.Video_Start (VIDEO_INPUT_CH);
+    if (error != DisplayBase::GRAPHICS_OK) {
+        printf("Line %d, error %d\n", __LINE__, error);
+        while (1);
+    }
+ 
+    /* Wait vsync to update resister */
+    WaitVsync(1);
+ 
+    /* Wait 2 Vfield(Top or bottom field) */
+    WaitVfield(2);
+ 
+    /* Initialize MCU functions */
+    init_MTU2_PWM_Motor();
+    init_MTU2_PWM_Servo();
+    interrput.attach(&intTimer, 0.001);
+    pc.baud(230400);
+ 
+    /* Initialize Micon Car state */
+    led_out( 0x0 );
+    handle( 0 );
+    motor( 0, 0 );
+ 
+    /* wait to stabilize NTSC signal (about 170ms) */
+    wait(0.2);
+ 
+    led_status_set( SENSOR );
+    CenterLine_Corrective( ImageBinary );
+    if( pushsw_get() ) {
+        while( 1 ){
+            ImageData_Serial_Out2( ImageBinary, 20 );
+        }
+    }
+    led_status_set( RUN );
+ 
+    while(1) {
+ 
+    switch( pattern ) {
+    /*****************************************************************
+    About patern
+     0:wait for switch input
+     1:check if start bar is open
+    11:normal trace
+    *****************************************************************/
+    case 0:
+        /* wait for switch input */
+        if( pushsw_get() ) {
+            led_out( 0x0 );
+            led_status_set( RUN );
+            pattern = 11;
+            cnt1 = 0;
+            break;
+        }
+        if( cnt1 < 100 ) {
+            led_out( 0x1 );
+        } else if( cnt1 < 200 ) {
+            led_out( 0x2 );
+        } else {
+            cnt1 = 0;
+        }
+        break;
+ 
+    case 11:
+        /* normal trace */
+        if( MarkCheck_Triangle( 85 ) ) {
+            led_status_set( MARK_T );
+            pattern = 91;
+            break;
+        }
+        switch( (digital_sensor()&0x0f) ) {
+            case 0x00:
+                handle( 0 );
+                motor( 100, 100 );
+                break;
+            case 0x02:
+                handle( 3 );
+                motor( 100, diff(100) );
+                break;
+            case 0x03:
+                handle( 12 );
+                motor( 100, diff(100) );
+                break;
+            case 0x01:
+                handle( 20 );
+                motor( 100, diff(100) );
+                pattern = 12;
+                break;
+            case 0x04:
+                handle( -3 );
+                motor( diff(100), 100 );
+                break;
+            case 0x0c:
+                handle( -12 );
+                motor( diff(100), 100 );
+                break;
+            case 0x08:
+                handle( -20 );
+                motor( diff(100), 100 );
+                pattern = 13;
+                break;
+            default:
+                break;
+        }
+        break;
+    
+    case 12:
+        /* Left side */
+        if( (digital_sensor()&0x02) == 0x02 ) {
+            pattern = 11;
+            break;
+        }
+        switch( (digital_sensor()&0x0f) ) {
+            case 0x01:
+                handle( 20 );
+                motor( 100, diff(100) );
+                break;
+            case 0x00:
+            case 0x08:
+            case 0x0c:
+                handle( 22 );
+                motor( 100, diff(100) );
+                break;
+            default:
+                break;
+        }
+        break;
+ 
+    case 13:
+        /* right side */
+        if( (digital_sensor()&0x04) == 0x04 ) {
+            pattern = 11;
+        }
+        switch( (digital_sensor()&0x0f) ) {
+            case 0x08:
+                handle( -20 );
+                motor( diff(100), 100 );
+                break;
+            case 0x00:
+            case 0x01:
+            case 0x03:
+                handle( -22 );
+                motor( diff(100), 100 );
+                break;
+            default:
+                break;
+        }
+        break;
+ 
+    case 91:
+        Brake_cnt = 0;
+        pattern = 92;
+        break;
+ 
+    case 92:
+        switch( Brake_cnt ) {
+            case 0:
+                cnt1 = 0;
+                Brake_cnt++;
+                break;
+            case 1:
+                motor( -90, -90 );
+                if( cnt1 > 85) Brake_cnt++;
+                break;
+            case 2:
+                motor( 0, 0 );
+                if( cnt1 > 100 ) Brake_cnt++;
+                break;
+            case 3:
+                motor( -60, -60 );
+                if( cnt1 > 185) Brake_cnt++;
+                break;
+            case 4:
+                motor( 0, 0 );
+                if( cnt1 > 200) Brake_cnt++;
+                break;
+            case 5:
+                motor( -40, -40 );
+                if( cnt1 > 285) Brake_cnt++;
+                break;
+            default:
+                motor( 0, 0 );
+                break;
+        }
+        break;
+ 
+    default:
+        break;
+    }
+    }
+}
+ 
+//Initialize MTU2 PWM functions
+//------------------------------------------------------------------//
+//MTU2_3, MTU2_4
+//Reset-Synchronized PWM mode
+//TIOC4A(P4_4) :Left-motor
+//TIOC4B(P4_5) :Right-motor
+//------------------------------------------------------------------//
+void init_MTU2_PWM_Motor( void )
+{
+    /* Port setting for S/W I/O Contorol */
+    /* alternative mode     */
+ 
+    /* MTU2_4 (P4_4)(P4_5)  */
+    GPIOPBDC4   = 0x0000;               /* Bidirection mode disabled*/
+    GPIOPFCAE4 &= 0xffcf;               /* The alternative function of a pin */
+    GPIOPFCE4  |= 0x0030;               /* The alternative function of a pin */
+    GPIOPFC4   &= 0xffcf;               /* The alternative function of a pin */
+                                        /* 2nd altemative function/output   */
+    GPIOP4     &= 0xffcf;               /*                          */
+    GPIOPM4    &= 0xffcf;               /* p4_4,P4_5:output         */
+    GPIOPMC4   |= 0x0030;               /* P4_4,P4_5:double         */
+ 
+    /* Mosule stop 33(MTU2) canceling */
+    CPGSTBCR3  &= 0xf7;
+ 
+    /* MTU2_3 and MTU2_4 (Motor PWM) */
+    MTU2TCR_3   = 0x20;                 /* TCNT Clear(TGRA), P0φ/1  */
+    MTU2TOCR1   = 0x04;                 /*                          */
+    MTU2TOCR2   = 0x40;                 /* N L>H  P H>L             */
+    MTU2TMDR_3  = 0x38;                 /* Buff:ON Reset-Synchronized PWM mode */
+    MTU2TMDR_4  = 0x30;                 /* Buff:ON                  */
+    MTU2TOER    = 0xc6;                 /* TIOC3B,4A,4B enabled output */
+    MTU2TCNT_3  = MTU2TCNT_4 = 0;       /* TCNT3,TCNT4 Set 0        */
+    MTU2TGRA_3  = MTU2TGRC_3 = MOTOR_PWM_CYCLE;
+                                        /* PWM-Cycle(1ms)           */
+    MTU2TGRA_4  = MTU2TGRC_4 = 0;       /* Left-motor(P4_4)         */
+    MTU2TGRB_4  = MTU2TGRD_4 = 0;       /* Right-motor(P4_5)        */
+    MTU2TSTR   |= 0x40;                 /* TCNT_4 Start             */
+}
+ 
+//Initialize MTU2 PWM functions
+//------------------------------------------------------------------//
+//MTU2_0
+//PWM mode 1
+//TIOC0A(P4_0) :Servo-motor
+//------------------------------------------------------------------//
+void init_MTU2_PWM_Servo( void )
+{
+    /* Port setting for S/W I/O Contorol */
+    /* alternative mode     */
+ 
+    /* MTU2_0 (P4_0)        */
+    GPIOPBDC4   = 0x0000;               /* Bidirection mode disabled*/
+    GPIOPFCAE4 &= 0xfffe;               /* The alternative function of a pin */
+    GPIOPFCE4  &= 0xfffe;               /* The alternative function of a pin */
+    GPIOPFC4   |= 0x0001;               /* The alternative function of a pin */
+                                        /* 2nd alternative function/output   */
+    GPIOP4     &= 0xfffe;               /*                          */
+    GPIOPM4    &= 0xfffe;               /* p4_0:output              */
+    GPIOPMC4   |= 0x0001;               /* P4_0:double              */
+ 
+    /* Mosule stop 33(MTU2) canceling */
+    CPGSTBCR3  &= 0xf7;
+ 
+    /* MTU2_0 (Motor PWM) */
+    MTU2TCR_0   = 0x22;                 /* TCNT Clear(TGRA), P0φ/16 */
+    MTU2TIORH_0 = 0x52;                 /* TGRA L>H, TGRB H>L       */
+    MTU2TMDR_0  = 0x32;                 /* TGRC and TGRD = Buff-mode*/
+                                        /* PWM-mode1                */
+    MTU2TCNT_0  = 0;                    /* TCNT0 Set 0              */
+    MTU2TGRA_0  = MTU2TGRC_0 = SERVO_PWM_CYCLE;
+                                        /* PWM-Cycle(16ms)          */
+    MTU2TGRB_0  = MTU2TGRD_0 = 0;       /* Servo-motor(P4_0)        */
+    MTU2TSTR   |= 0x01;                 /* TCNT_0 Start             */
+}
+ 
+//Interrupt Timer
+//------------------------------------------------------------------//
+void intTimer( void )
+{
+    int         x, y;
+    static int  counter = 0;
+ 
+    cnt0++;
+    cnt1++;
+ 
+    /* Mark Check Process */
+    switch( counter++ ) {
+    case 0:
+        change_framebuffer_process();
+        break;
+    case 1:
+        Image_Extraction( ImageData );
+        break;
+    case 2:
+        Image_Extraction( ImageData );
+        break;
+    case 3:
+        Image_Extraction( ImageData );
+        break;
+    case 4:
+        Image_Extraction( ImageData );
+        break;
+    case 5:
+        Image_Extraction( ImageData );
+        break;
+    case 6:
+        Image_Extraction( ImageData );
+        break;
+    case 7:
+        Image_Extraction( ImageData );
+        break;
+    case 8:
+        Image_Extraction( ImageData );
+        break;
+    case 9:
+        Image_Compression2( ImageData, 320, ImageComp, 16 );
+        break;
+    case 10:
+        Image_Compression2( ImageData, 320, ImageComp, 16 );
+        break;
+    case 11:
+        Binarization_process( ImageComp, ImageBinary, 20*15 );
+        break;
+    case 12:
+        retDevi_Triangle  = Standard_Deviation( TempBinary_Triangle, TempDevi_Triangle, 15 );
+        break;
+    case 13:
+        //▽Triangle
+        retJudgeIM_Max[0] = 0;
+        for( y = 0; y <= 12; y++ ) {
+            for( x = 0; x <= 15; x++ ) {
+                Image_part_Extraction( ImageBinary, 20, x, y, NowImageBinary, 5, 3 );
+                retDevi    = Standard_Deviation( NowImageBinary, NowDevi, 15 );
+                retCovari  = Covariance( TempDevi_Triangle, NowDevi, 15 );
+                retJudgeIM = 0;
+                retJudgeIM = Judgement_ImageMatching( retCovari, retDevi_Triangle, retDevi );
+                if( 100 >= retJudgeIM && retJudgeIM > retJudgeIM_Max[0] ) {
+                    Xt = x;
+                    Yt = y;
+                    retJudgeIM_Max[0] = retJudgeIM;
+                }
+            }
+        }
+        break;
+    case 33:
+        counter = 0;
+        break;
+    default:
+        break;
+    }
+ 
+    /* Trace by image processing */
+    digital_sensor_process( ImageBinary );
+ 
+    /* LED processing */
+    led_status_process();
+}
+ 
+//LED_RGB(on GR-PEACH board)
+//------------------------------------------------------------------//
+void led_rgb(int led)
+{
+    LED_R = led & 0x1;
+    LED_G = (led >> 1 ) & 0x1;
+    LED_B = (led >> 2 ) & 0x1;
+}
+ 
+//user_button_get(on GR-PEACH board)
+//------------------------------------------------------------------//
+unsigned int user_button_get( void )
+{
+    return (~user_botton) & 0x1;        /* Read ports with switches */
+}
+ 
+//LED_Status(on GR-PEACH board) Function for only interrupt
+//------------------------------------------------------------------//
+void led_status_process( void )
+{
+    static unsigned long    led_timer;
+    int                     led_set;
+    int                     on_time;
+    int                     off_time;
+ 
+    /* setting */
+    switch( status_set ){
+    case RUN:
+        led_set  = LED_GREEN;
+        on_time  = 500;
+        off_time = 500;
+        break;
+ 
+    case SENSOR:
+        led_set  = LED_BLUE;
+        on_time  = 50;
+        off_time = 50;
+        break;
+ 
+    case MARK_T:
+        led_set  = LED_WHITE;
+        on_time  = 250;
+        off_time = 250;
+        break;
+ 
+    case MARK_C:
+        led_set  = LED_YELLOW;
+        on_time  = 250;
+        off_time = 250;
+        break;
+ 
+    case MARK_R:
+        led_set  = LED_PURPLE;
+        on_time  = 250;
+        off_time = 250;
+        break;
+ 
+    case MARK_L:
+        led_set  = LED_SKYBLUE;
+        on_time  = 250;
+        off_time = 250;
+        break;
+ 
+    case STOP:
+        led_set  = LED_RED;
+        on_time  = 1;
+        off_time = 0;
+        break;
+ 
+    case ERROR:
+        led_set  = LED_RED;
+        on_time  = 50;
+        off_time = 50;
+        break;
+ 
+    default:
+        led_set  = LED_OFF;
+        on_time  = 0;
+        off_time = 1;
+        break;
+    }
+ 
+    /* Display */
+    led_timer++;
+    if( led_timer < on_time ) led_rgb( led_set );
+    else if( led_timer < ( on_time + off_time ) ) led_rgb( LED_OFF );
+    else led_timer = 0;
+}
+ 
+//LED_Status(on GR-PEACH board) Function for only interrupt
+//------------------------------------------------------------------//
+void led_status_set( int set )
+{
+    status_set = set;
+}
+ 
+//led_out(on Motor drive board)
+//------------------------------------------------------------------//
+void led_out(int led)
+{
+    led = ~led;
+    LED_3 = led & 0x1;
+    LED_2 = ( led >> 1 ) & 0x1;
+}
+ 
+//pushsw_get(on Motor drive board)
+//------------------------------------------------------------------//
+unsigned int pushsw_get( void )
+{
+    return (~push_sw) & 0x1;            /* Read ports with switches */
+}
+ 
+//motor speed control(PWM)
+//Arguments: motor:-100 to 100
+//Here, 0 is stop, 100 is forward, -100 is reverse
+//------------------------------------------------------------------//
+void motor( int accele_l, int accele_r )
+{
+    accele_l = ( accele_l * MAX_SPEED ) / 100;
+    accele_r = ( accele_r * MAX_SPEED ) / 100;
+ 
+    /* Left Motor Control */
+    if( accele_l >= 0 ) {
+        /* forward */
+        Left_motor_signal = 0;
+        MTU2TGRC_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * accele_l / 100;
+    } else {
+        /* reverse */
+        Left_motor_signal = 1;
+        MTU2TGRC_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * ( -accele_l ) / 100;
+    }
+ 
+    /* Right Motor Control */
+    if( accele_r >= 0 ) {
+        /* forward */
+        Right_motor_signal = 0;
+        MTU2TGRD_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * accele_r / 100;
+    } else {
+        /* reverse */
+        Right_motor_signal = 1;
+        MTU2TGRD_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * ( -accele_r ) / 100;
+    }
+}
+ 
+//Handle fanction
+//------------------------------------------------------------------//
+void handle( int angle )
+{
+    handle_buff = angle;
+    /* When the servo move from left to right in reverse, replace "-" with "+" */
+    MTU2TGRD_0 = SERVO_CENTER - angle * HANDLE_STEP;
+}
+ 
+//handle(on Motor drive board)
+//------------------------------------------------------------------//
+int diff( int pwm )
+{
+    int i, ret;
+ 
+    i  = handle_buff;
+    if( i <  0 ) i = -i;
+    if( i > 45 ) i = 45;
+    ret = revolution_difference[i] * pwm / 100;
+ 
+    return ret;
+}
+ 
+// MarkCheck Triangle detection
+// Return values: 0: no triangle mark, 1: Triangle mark
+//------------------------------------------------------------------//
+int MarkCheck_Triangle( int percentage )
+{
+    int ret;
+ 
+    ret = 0;
+    if( retJudgeIM_Max[0] >= percentage ) {
+        ret = 1;
+    }
+    return ret;
+}
+ 
+//Image Data Output( for the Excel )
+//------------------------------------------------------------------//
+void ImageData_Serial_Out( unsigned char *Data_Y, int Width )
+{
+    int     Xp, Yp, inc, Height;
+ 
+    Height = (Width / (double)4) * 3;
+    for( Yp = 0, inc = 0; Yp < Height; Yp++ ) {
+        for( Xp = 0; Xp < Width; Xp++, inc++ ) {
+            pc.printf( "%d,", Data_Y[ inc ] );
+        }
+        pc.printf("\n\r");
+    }
+}
+ 
+//Image Data Output2( for TeraTerm )
+//------------------------------------------------------------------//
+void ImageData_Serial_Out2( unsigned char *Data_Y, int Width )
+{
+    int     Xp, Yp, Height;
+ 
+    Height = (Width / (double)4) * 3;
+    for( Yp = 0; Yp < Height; Yp++ ) {
+        for( Xp = 0; Xp < Width; Xp++ ) {
+            pc.printf( "%d ", Data_Y[Xp + (Yp * Width)] );
+        }
+        pc.printf( "\n\r" );
+    }
+ 
+#if 1
+    //Add display
+    pc.printf( "\n\r" );
+    pc.printf( "T= %3d%% %01d X=%2d Y=%2d\n\r", retJudgeIM_Max[0], MarkCheck_Triangle( 90 ), Xt, Yt );
+    pc.printf( "\n\r" );
+    Height += 3;
+#endif
+ 
+    pc.printf( "\033[%dA" , Height );
+}
+ 
+//Image Data Output3( for the converter ( csv --> jpg ) )
+//------------------------------------------------------------------//
+void ImageData_Serial_Out3( void )
+{
+    int     Xp, Yp, x, y;
+ 
+    /* Camera module test process */
+    pc.printf( "//,X-Size,Y-Size" );
+    pc.printf( "\n\r" );
+    pc.printf( "#SIZE,320,240" );
+    pc.printf( "\n\r" );
+    pc.printf( "//,X-Point,Y-Point" );
+    pc.printf( "\n\r" );
+ 
+    for( Yp = 0, y = 0; Yp < 240; Yp+=1, y++ ){
+        for( Xp = 0, x = 0; Xp < 640; Xp+=4, x+=2 ){
+            pc.printf( "#YCbCr," );
+      /*Xp*/pc.printf( "%d,", x);
+      /*Yp*/pc.printf( "%d,", y);
+      /*Y0*/pc.printf( "%d,", save_buff_addr[(Xp+0)+(640*Yp)]);//6
+      /*Cb*/pc.printf( "%d,", save_buff_addr[(Xp+1)+(640*Yp)]);//5
+      /*Cr*/pc.printf( "%d,", save_buff_addr[(Xp+3)+(640*Yp)]);//7
+            pc.printf( "\n\r" );
+ 
+            pc.printf( "#YCbCr," );
+      /*Xp*/pc.printf( "%d,", x+1);
+      /*Yp*/pc.printf( "%d,", y);
+      /*Y1*/pc.printf( "%d,", save_buff_addr[(Xp+2)+(640*Yp)]);//4
+      /*Cb*/pc.printf( "%d,", save_buff_addr[(Xp+1)+(640*Yp)]);//5
+      /*Cr*/pc.printf( "%d,", save_buff_addr[(Xp+3)+(640*Yp)]);//7
+            pc.printf( "\n\r" );
+        }
+    }
+}
+ 
+//Change FrameBuffer Process
+//------------------------------------------------------------------//
+void change_framebuffer_process( void )
+{
+    DisplayBase::graphics_error_t error;
+    DisplayBase Display;
+ 
+    /* Change address buffer */
+    if (write_buff_addr == FrameBuffer_Video_A) {
+        write_buff_addr = FrameBuffer_Video_B;
+        save_buff_addr  = FrameBuffer_Video_A;
+    } else {
+        write_buff_addr = FrameBuffer_Video_A;
+        save_buff_addr  = FrameBuffer_Video_B;
+    }
+ 
+    /* Change write buffer */
+    error = Display.Video_Write_Change(
+                VIDEO_INPUT_CH,
+                write_buff_addr,
+                VIDEO_BUFFER_STRIDE);
+    if (error != DisplayBase::GRAPHICS_OK) {
+        printf("Line %d, error %d\n", __LINE__, error);
+        while (1);
+    }
+}
+ 
+//CenterLine_Corrective  image size 20*15pix
+//------------------------------------------------------------------//
+int CenterLine_Corrective( unsigned char *Binary )
+{
+    #define L       0
+    #define R       1
+ 
+    int iRet, offset_X, offset_Y;
+    int Xpix, X;
+    int Ypix;
+    int Pixel_diff[2];
+    int Error_cnt;
+    int value;
+ 
+    /* Center of image */
+    offset_X = 6;
+    offset_Y = 12;
+ 
+    /* corrective of center line */
+    for( Ypix = 0, Error_cnt = 0; Ypix < (offset_Y - 4); Ypix++ ) {
+        for( value = 0; value < 2; value++ ) {
+            for( Xpix = offset_X; Xpix < (offset_X + 8); Xpix++ ) {
+                /* Lift side */
+                Pixel_diff[L] = 0;
+                if( Binary[ ( ( offset_Y - Ypix ) * 20 ) + Xpix ] >= 1 ) {
+                    for( X = Xpix; X > (Xpix - 4); X-- ) {
+                        if( Binary[ ( ( offset_Y - Ypix ) * 20 ) + X ] >= 1 ) {
+                            Pixel_diff[L]++;
+                        } else {
+                            break;
+                        }
+                    }
+                } else {
+                    Pixel_diff[L] = -1;
+                }
+                /* Right side */
+                Pixel_diff[R] = 0;
+                if( Binary[ ( ( offset_Y - Ypix ) * 20 ) + (Xpix + 1) ] >= 1 ) {
+                    for( X = (Xpix + 1); X < ((Xpix + 1) + 4); X++ ) {
+                        if( Binary[ ( ( offset_Y - Ypix ) * 20 ) + X ] >= 1 ) {
+                            Pixel_diff[R]++;
+                        } else {
+                            break;
+                        }
+                    }
+                } else {
+                    Pixel_diff[R] = 1;
+                }
+                /* check */
+                iRet = Pixel_diff[L] - Pixel_diff[R];
+                if( value >= iRet && iRet >= -value ) {
+                    break;
+                }
+            }
+            if( value >= iRet && iRet >= -value ) {
+                /* X coordinate */
+                Sensor_X[Ypix][2] = Xpix;
+                Sensor_X[Ypix][3] = Xpix + 1;
+                break;
+            } else {
+                Sensor_X[Ypix][2] = Sensor_X[Ypix][3] = -1;
+                Error_cnt++;
+            }
+        }
+        /* Left side sensor */
+        Sensor_X[Ypix][1] = Sensor_X[Ypix][2] - ( Pixel_diff[L] );
+        Sensor_X[Ypix][0] = Sensor_X[Ypix][1] - ( Pixel_diff[L] + 1 );//( Sensor_X[Ypix][2] - Sensor_X[Ypix][1] );
+        /* Right side sensor */
+        Sensor_X[Ypix][4] = Sensor_X[Ypix][3] + ( Pixel_diff[R] );
+        Sensor_X[Ypix][5] = Sensor_X[Ypix][4] + ( Pixel_diff[R] + 1 );//( Sensor_X[Ypix][4] - Sensor_X[Ypix][3] );
+    }
+    return Error_cnt;
+}
+ 
+//digital_sensor_process
+//------------------------------------------------------------------//
+void digital_sensor_process( unsigned char *Binary )
+{
+    static int      counter = 0;
+    static int      Ypix = 0;
+ 
+    int             offset_Y;
+    unsigned char   sensor, data;
+ 
+    offset_Y    = 12;
+    sensor      = 0;
+ 
+    // Distributed processing
+    switch( counter++ ) {
+    case 15:
+    case 31:
+        data  = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][2] ] & 0x01;
+        data |= Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][3] ] & 0x01;
+        sensor |= (data << 4) & 0x10;
+        data  = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][0] ] & 0x01;
+        sensor |= (data << 3) & 0x08;
+        data  = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][1] ] & 0x01;
+        sensor |= (data << 2) & 0x04;
+        data  = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][4] ] & 0x01;
+        sensor |= (data << 1) & 0x02;
+        data  = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][5] ] & 0x01;
+        sensor |= (data << 0) & 0x01;
+        sensor &= 0x1f;
+        sensor_value = sensor;
+        Ypix += 4;
+        break;
+    case 33:
+        counter = 0;
+        Ypix    = 0;
+        break;
+    default:
+        break;
+    }
+}
+ 
+//digital_sensor
+//------------------------------------------------------------------//
+unsigned char digital_sensor( void )
+{
+    return sensor_value;
+}
+ 
+//Image Data YCbCr -> Y(320*240pix)
+//------------------------------------------------------------------//
+void Image_Extraction( unsigned char *Data_Y )
+{
+    static int  Xp, Yp, inc;
+    static int  counter = 0;
+ 
+    // Distributed processing
+    switch( counter++ ) {
+    case 0:
+        for( Yp = 0, inc = 0; Yp < 30; Yp++ ){
+            for( Xp = 0; Xp < 640; Xp+=2, inc++ ){
+                Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)];
+            }
+        }
+        break;
+    case 1:
+        for( ; Yp < 60; Yp++ ){
+            for( Xp = 0; Xp < 640; Xp+=2, inc++ ){
+                Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)];
+            }
+        }
+        break;
+    case 2:
+        for( ; Yp < 90; Yp++ ){
+            for( Xp = 0; Xp < 640; Xp+=2, inc++ ){
+                Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)];
+            }
+        }
+        break;
+    case 3:
+        for( ; Yp < 120; Yp++ ){
+            for( Xp = 0; Xp < 640; Xp+=2, inc++ ){
+                Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)];
+            }
+        }
+        break;
+    case 4:
+        for( ; Yp < 150; Yp++ ){
+            for( Xp = 0; Xp < 640; Xp+=2, inc++ ){
+                Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)];
+            }
+        }
+        break;
+    case 5:
+        for( ; Yp < 180; Yp++ ){
+            for( Xp = 0; Xp < 640; Xp+=2, inc++ ){
+                Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)];
+            }
+        }
+        break;
+    case 6:
+        for( ; Yp < 210; Yp++ ){
+            for( Xp = 0; Xp < 640; Xp+=2, inc++ ){
+                Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)];
+            }
+        }
+        break;
+    case 7:
+        for( ; Yp < 240; Yp++ ){
+            for( Xp = 0; Xp < 640; Xp+=2, inc++ ){
+                Data_Y[ inc ] = save_buff_addr[(Xp)+(640*Yp)];
+            }
+        }
+        counter = 0;
+        break;
+    default:
+        break;
+    }
+ 
+}
+ 
+//Image_Compression Y ( Thinning processing )
+//------------------------------------------------------------------//
+void Image_Compression( unsigned char *Data_Y, int Data_W , unsigned char *Comp_Y, int Comp_W )
+{
+    int  Data_H, Comp_H, Step_W, Step_H;
+    int  Xp, Yp, inc;
+ 
+    Data_H = (Data_W / (double)4) * 3;
+    Comp_H = (Comp_W / (double)4) * 3;
+    Step_W = Data_W / Comp_W;
+    Step_H = Data_H / Comp_H;
+ 
+    for( Yp = 0, inc = 0; Yp < Comp_H; Yp++ ) {
+        for( Xp = 0; Xp < Comp_W; Xp++, inc++ ) {
+            Comp_Y[inc] = Data_Y[(Yp * (Data_W * Step_H)) + (Xp * Step_W)];
+        }
+    }
+}
+ 
+//Image_Compression2 Y ( Averaging processing )
+//------------------------------------------------------------------//
+void Image_Compression2( unsigned char *Data_Y, int Data_W , unsigned char *Comp_Y, int Comp_M )
+{
+    int         Data_H, Pixel_T, Pixel_D;
+    int         x, y;
+    static int  Xp, Yp, inc;
+    static int  counter = 0;
+ 
+    Data_H  = (Data_W / (double)4) * 3;
+    Pixel_D = Comp_M * Comp_M;
+ 
+    // Distributed processing
+    switch( counter++ ) {
+    case 0:
+        for( Yp = 0, inc = 0; Yp < (Data_H / 2); Yp+=Comp_M ){
+            for( Xp = 0; Xp < Data_W; Xp+=Comp_M, inc++ ){
+                Pixel_T = 0;            
+                for( y = 0; y < Comp_M; y++ ){
+                    for( x = 0; x < Comp_M; x++ ){
+                        Pixel_T += Data_Y[( Xp + x ) + (( Yp + y ) * Data_W )];
+                    }
+                }
+                Comp_Y[inc] = Pixel_T / Pixel_D;
+            }
+        }
+        break;
+    case 1:
+        for( ; Yp < Data_H; Yp+=Comp_M ){
+            for( Xp = 0; Xp < Data_W; Xp+=Comp_M, inc++ ){
+                Pixel_T = 0;            
+                for( y = 0; y < Comp_M; y++ ){
+                    for( x = 0; x < Comp_M; x++ ){
+                        Pixel_T += Data_Y[( Xp + x ) + (( Yp + y ) * Data_W )];
+                    }
+                }
+                Comp_Y[inc] = Pixel_T / Pixel_D;
+            }
+        }
+        counter = 0;
+        break;
+    default:
+        break;
+    }
+ 
+}
+ 
+// Binarization_process
+//------------------------------------------------------------------//
+void Binarization_process( unsigned char *Comp_Y, unsigned char *Binary, long items )
+{
+    int     i, threshold;
+ 
+    threshold = 128;
+    for( i = 0; i < items; i++ ) {
+        if( Comp_Y[i] >= threshold )   Binary[i] = 1;
+        else                           Binary[i] = 0;
+    }
+}
+ 
+// Extract_Image
+//------------------------------------------------------------------//
+void Image_part_Extraction( unsigned char *Binary, int Width, int Xpix, int Ypix, unsigned char *Data_B, int x_size, int y_size )
+{
+    int     x, y;
+    for( y = 0; y < y_size; y++ ) {
+        for( x = 0; x < x_size; x++ ) {
+            Data_B[ x + ( y * x_size ) ] = Binary[ (Xpix + x) + ( (Ypix + y) * Width ) ];
+       }
+    }
+}
+ 
+// Standard deviation
+//------------------------------------------------------------------//
+double Standard_Deviation( unsigned char *data, double *Devi, int items )
+{
+    int         i;
+    double      iRet_A, iRet_C, iRet_D;
+ 
+    /* A 合計値　平均化 */
+    iRet_A = 0;
+    for( i = 0; i < items; i++ ) {
+        iRet_A += data[i];
+    }
+    iRet_A /= items;
+ 
+    /* B 偏差値 */
+    for( i = 0; i < items; i++ ) {
+        Devi[i] = data[i] - iRet_A;
+    }
+ 
+    /* C 分散 */
+    iRet_C = 0;
+    for( i = 0; i < items; i++ ) {
+        iRet_C += ( Devi[i] * Devi[i] );
+    }
+    iRet_C /= items;
+ 
+    /* D 標準偏差 */
+    iRet_D = sqrt( iRet_C );
+ 
+    return iRet_D;
+}
+ 
+// Covariance
+//------------------------------------------------------------------//
+double Covariance( double *Devi_A, double *Devi_B, int items )
+{
+    int     i;
+    double  iRet, iRet_buff;
+ 
+    iRet = 0;
+    for( i = 0; i < items; i++ ) {
+        iRet_buff = Devi_A[i] * Devi_B[i];
+        iRet     += iRet_buff;
+    }
+    iRet /= items;
+ 
+    return iRet;
+}
+ 
+// Judgement_ImageMatching
+//------------------------------------------------------------------//
+int Judgement_ImageMatching( double covari, double SDevi_A, double SDevi_B )
+{
+    int     iRet;
+ 
+    iRet  = ( covari * 100 ) / ( SDevi_A * SDevi_B );
+ 
+    return iRet;
+}
+ 
+//******************************************************************//
+// @brief       Interrupt callback function
+// @param[in]   int_type    : VDC5 interrupt type
+// @retval      None
+//*******************************************************************/
+static void IntCallbackFunc_Vfield(DisplayBase::int_type_t int_type)
+{
+    if (vfield_count > 0) {
+        vfield_count--;
+    }
+}
+ 
+//******************************************************************//
+// @brief       Wait for the specified number of times Vsync occurs
+// @param[in]   wait_count          : Wait count
+// @retval      None
+//*******************************************************************/
+static void WaitVfield(const int32_t wait_count)
+{
+    vfield_count = wait_count;
+    while (vfield_count > 0) {
+        /* Do nothing */
+    }
+}
+ 
+//******************************************************************//
+// @brief       Interrupt callback function for Vsync interruption
+// @param[in]   int_type    : VDC5 interrupt type
+// @retval      None
+//*******************************************************************/
+static void IntCallbackFunc_Vsync(DisplayBase::int_type_t int_type)
+{
+    if (vsync_count > 0) {
+        vsync_count--;
+    }
+}
+ 
+//******************************************************************//
+// @brief       Wait for the specified number of times Vsync occurs
+// @param[in]   wait_count          : Wait count
+// @retval      None
+//*******************************************************************/
+static void WaitVsync(const int32_t wait_count)
+{
+    vsync_count = wait_count;
+    while (vsync_count > 0) {
+        /* Do nothing */
+    }
+}
+ 
+//------------------------------------------------------------------//
+// End of file
+//------------------------------------------------------------------//
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Sep 02 07:34:13 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/2241e3a39974
\ No newline at end of file