File content as of revision 0:158dbfb66e62:

//------------------------------------------------------------------//
//Supported MCU:   RZ/A1H
//File Contents:   Trace Program by Image Processing
//                                 (GR-PEACH version on the Micon Car)
//Version number:  Ver.1.00
//Date:            2016.09.22
//Copyright:       Renesas Electronics Corporation
//                 Hitachi Document Solutions Co., Ltd.
//------------------------------------------------------------------//

//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           40      /* 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     ERROR               0x00
#define     STOP                0x01
#define     RUN                 0x02
#define     DEBUG               0x03
#define     MOTOR_START         0x04
#define     MOTOR_STOP          0x05
#define     MARK_T              0x06
 
//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 */
DigitalOut  USER_LED(P6_12);            /* USER_LED 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(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
//------------------------------------------------------------------//
//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);
void led_m_user( int led );
unsigned int user_button_get( void );
void led_m_set( int set );
void led_m_process( void );             /* Function for only interrupt */
 
//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 );
 
//Prototype(Image process)
//------------------------------------------------------------------//
void Image_Extraction( unsigned char *buff_addr, unsigned char *Data_Y, int frame );
void Image_Reduction( 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, int threshold );

//Prototype(Digital sensor process)
//------------------------------------------------------------------//
int CenterLine_Corrective( unsigned char *Binary );
void digital_sensor_process( unsigned char *Binary );   /* Function for only interrupt */
unsigned char digital_sensor( void );

//Prototype(Mark detection process)
//------------------------------------------------------------------//
void Image_part_Extraction( unsigned char *Binary, int Width, int Xpix, int Ypix, unsigned char *Data_B, int x_size, int y_size );
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 );
void MarkDetect_process_T( void );
int MarkCheck_Triangle( int percentage );

//Prototype(Display Debug)
//------------------------------------------------------------------//
void ImageData_Serial_Out( unsigned char *Data_Y, int Width );
void ImageData_Serial_Out2( unsigned char *Data_Y, int Width );

//Globle variable (NTSC-video)
//------------------------------------------------------------------//
static uint8_t FrameBuffer_Video_A[VIDEO_BUFFER_STRIDE * VIDEO_BUFFER_HEIGHT]__attribute((section("NC_BSS"),aligned(16)));  //16 bytes aligned!;
uint8_t * write_buff_addr = FrameBuffer_Video_A;
static volatile int32_t vsync_count;
static volatile int32_t vfield_count;
static volatile int32_t vfield_count2 = 1;
static volatile int32_t vfield_count2_buff;

//Globle variable for Image process
//------------------------------------------------------------------//
unsigned char   ImageData_A[160*120];
unsigned char   ImageComp_A[20*15];
unsigned char   ImageBinary[20*15];

//Globle variable for Digital sensor process
//------------------------------------------------------------------//
volatile int            Sensor_X[8][6];
volatile unsigned char  sensor_value;

//Globle variable for Mark detection process
//------------------------------------------------------------------//
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[15];
unsigned char   NowImageBinary[15];

volatile double retDevi_Triangle;

volatile double retDevi;
volatile double retCovari;
volatile int    retJudgeIM;
volatile int    retJudgeIM_Max[1];

int             Xt, Yt;

//Globle variable for led fanction
//------------------------------------------------------------------//
volatile int    led_set;                /* Status                   */

                               // LED,  OnTime,  OffTime,
volatile int    led_data[10][3]= {LED_RED,     50,    50,   /* ERROR  */
                                  LED_RED,    500,     0,   /* STOP   */
                                  LED_GREEN,  500,   500,   /* RUN    */
                                  LED_BLUE,    50,    50,   /* DEBUG  */
                                  LED_GREEN,    1,     0,   /* MOTOR_START */
                                  LED_RED,      1,     0,   /* MOTOR_STOP */
                                  LED_WHITE,  500,   500};  /* MARK_T */

//Globle variable for Trace program
//------------------------------------------------------------------//
volatile unsigned long  cnt0;           /* Used by timer function   */
volatile unsigned long  cnt1;           /* Used within main         */
volatile int            pattern;        /* Pattern numbers          */
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 };

//******************************************************************//
// Main function
//*******************************************************************/
int main( void )
{
    /* 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);

    /* Initialize Digital sensor */
    CenterLine_Corrective( ImageBinary );

    /* Initialize Mark detection */
    retDevi_Triangle  = Standard_Deviation( TempBinary_Triangle, TempDevi_Triangle, 15 );

    if( user_button_get() ) {
        wait(0.1);
        led_m_set( DEBUG );
        while( user_button_get() );
        wait(0.5);
        pc.printf( "Please push the SW ( on the Motor drive board )\n\r" );
        pc.printf( "\n\r" );
        while( !user_button_get() );
        while( 1 ){
            ImageData_Serial_Out2( ImageBinary, 20 );
        }
    }

    led_m_set( RUN );
 
    while( 1 ) {
 
    switch( pattern ) {
    /*****************************************************************
    About patern
     0:wait for switch input
     1:check if start bar is open
    11:normal trace
    12:Left side
    13:right side
    *****************************************************************/
    case 0:
        /* wait for switch input */
        if( pushsw_get() ) {
            led_out( 0x0 );
            led_m_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( 90 ) ) {
            led_m_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:
        /* Stop */
        handle( 0 );
        motor( 0, 0 );
        break;

    default:
        break;
    }
    }
}

//******************************************************************//
// Initialize functions
//*******************************************************************/
//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             */
}

//******************************************************************//
// @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--;
    }
    /* top or bottom (Change) */
    if      ( vfield_count2 == 0 )  vfield_count2 = 1;
    else if ( vfield_count2 == 1 )  vfield_count2 = 0;
}
 
//******************************************************************//
// @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 */
    }
}

//******************************************************************//
// Interrupt function( intTimer )
//*******************************************************************/
void intTimer( void )
{
    static int  counter = 0;
 
    cnt0++;
    cnt1++;
 
    /* field check */
    if( vfield_count2 == vfield_count2_buff ) {
        vfield_count2_buff = vfield_count2;
    }
    /* Top field */
    if( !vfield_count2 ) {
        led_m_user( 1 );
        switch( counter++ ) {
        case 0:
            Image_Extraction( write_buff_addr, ImageData_A, vfield_count2 );
            break;
        case 1:
            Image_Extraction( write_buff_addr, ImageData_A, vfield_count2 );
            break;
        case 2:
            Image_Reduction( ImageData_A, 160, ImageComp_A, 8 );
            break;
        case 3:
            Image_Reduction( ImageData_A, 160, ImageComp_A, 8 );
            break;
        case 4:
            Binarization_process( ImageComp_A, ImageBinary, 20*15, 128 );
            break;
        case 5:
            /* Trace by image processing */
            digital_sensor_process( ImageBinary );
            break;
        case 6:
            //MarkCheck_Triangle
            MarkDetect_process_T();
            break;
        case 15:
            counter = 0;
            break;
        default:
            break;
        }
    }
    /* bottom field */
    else {
        led_m_user( 0 );
        switch( counter++ ) {
        case 0:
            Image_Extraction( write_buff_addr, ImageData_A, vfield_count2 );
            break;
        case 1:
            Image_Extraction( write_buff_addr, ImageData_A, vfield_count2 );
            break;
        case 2:
            Image_Reduction( ImageData_A, 160, ImageComp_A, 8 );
            break;
        case 3:
            Image_Reduction( ImageData_A, 160, ImageComp_A, 8 );
            break;
        case 4:
            Binarization_process( ImageComp_A, ImageBinary, 20*15, 128 );
            break;
        case 5:
            /* Trace by image processing */
            digital_sensor_process( ImageBinary );
            break;
        case 6:
            //MarkCheck_Triangle
            MarkDetect_process_T();
            break;
        case 15:
            counter = 0;
            break;
        default:
            break;
        }
    }

    /* LED processing */
    led_m_process();
}

//******************************************************************//
// functions ( on GR-PEACH board )
//*******************************************************************/
//led_rgb Function
//------------------------------------------------------------------//
void led_rgb(int led)
{
    LED_R = led & 0x1;
    LED_G = (led >> 1 ) & 0x1;
    LED_B = (led >> 2 ) & 0x1;
}
 
//user_button_get Function
//------------------------------------------------------------------//
unsigned int user_button_get( void )
{
    return (~user_botton) & 0x1;        /* Read ports with switches */
}

//led_m_user Function
//------------------------------------------------------------------//
void led_m_user( int led )
{
    USER_LED = led & 0x01;
}

//Lled_m_set Function
//------------------------------------------------------------------//
void led_m_set( int set )
{
    led_set = set;
}

//led_m_process Function for only interrupt
//------------------------------------------------------------------//
void led_m_process( void )
{
    static unsigned long    led_timer;

    led_timer++;

    /* Display */
    if( led_timer < led_data[led_set][1] ) led_rgb( led_data[led_set][0] );
    else if( led_timer < ( led_data[led_set][1] + led_data[led_set][2] ) ) led_rgb( LED_OFF );
    else led_timer = 0;
}

//******************************************************************//
// functions ( on Motor drive board )
//*******************************************************************/
//led_out Function
//------------------------------------------------------------------//
void led_out(int led)
{
    led = ~led;
    LED_3 = led & 0x1;
    LED_2 = ( led >> 1 ) & 0x1;
}
 
//pushsw_get Function
//------------------------------------------------------------------//
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 Function
//------------------------------------------------------------------//
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;
}
 
//diff Function
//------------------------------------------------------------------//
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;
}

//******************************************************************//
// Image process functions
//*******************************************************************/
//Image Data YCbCr -> Y(320*240pix) -> Y(160*120)
//frame 0 : Top field
//frame 1 : Bottom field
//------------------------------------------------------------------//
void Image_Extraction( unsigned char *buff_addr, unsigned char *Data_Y, int frame )
{
    static int  Xp, Yp, inc, Data_Y_buff;
    static int  counter = 0;

    // Distributed processing
    switch( counter++ ) {
    case 0:
        for( Yp = frame, inc = 0; Yp < 120; Yp+=2 ){
            for( Xp = 0; Xp < 640; Xp+=4, inc++ ){
                Data_Y_buff   = (int)buff_addr[(Xp+0)+(640*Yp)];
                Data_Y_buff  += (int)buff_addr[(Xp+2)+(640*Yp)];
                Data_Y[inc]   = Data_Y_buff >> 1;
            }
        }
        break;
    case 1:
        for(     /* None */     ; Yp < 240; Yp+=2 ){
            for( Xp = 0; Xp < 640; Xp+=4, inc++ ){
                Data_Y_buff   = (int)buff_addr[(Xp+0)+(640*Yp)];
                Data_Y_buff  += (int)buff_addr[(Xp+2)+(640*Yp)];
                Data_Y[inc]   = Data_Y_buff >> 1;
            }
        }
        counter = 0;
        break;
    default:
        break;
    }    
}
 
//Image_Reduction Function ( Averaging processing )
//------------------------------------------------------------------//
void Image_Reduction( 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;

    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(   /* None */   ; 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 Function
//------------------------------------------------------------------//
void Binarization_process( unsigned char *Comp_Y, unsigned char *Binary, long items, int threshold )
{
    int     i;
 
    for( i = 0; i < items; i++ ) {
        if( Comp_Y[i] >= threshold )   Binary[i] = 1;
        else                           Binary[i] = 0;
    }
}

//******************************************************************//
// digital sensor functions
//*******************************************************************/
//CenterLine_Corrective Function ( 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 Function
//------------------------------------------------------------------//
void digital_sensor_process( unsigned char *Binary )
{
    int             Ypix;
    int             offset_Y;
    unsigned char   sensor, data;
 
    offset_Y    = 12;
    sensor      = 0;
 
    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;
}
 
//digital_sensor Function
//------------------------------------------------------------------//
unsigned char digital_sensor( void )
{
    return sensor_value;
}

//******************************************************************//
// Mark detect functions
//*******************************************************************/
// 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;
}

// MarkDetect_process_T
//------------------------------------------------------------------//
void MarkDetect_process_T( void )
{
    int  x, y;

    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;
            }
        }
    }
}

// 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;
}

//******************************************************************//
// Debug functions
//*******************************************************************/
//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" );
    }

    //Add display
    pc.printf( "\n\r" );
    pc.printf( "sensor_inp = 0x%02x\n\r", digital_sensor() );
    pc.printf( "T = %3d%% %01d X=%2d Y=%2d\n\r", retJudgeIM_Max[0], MarkCheck_Triangle( 90 ),  Xt, Yt );
    pc.printf( "\n\r" );
    Height += 4;

    pc.printf( "\033[%dA" , Height );
}

//------------------------------------------------------------------//
// End of file
//------------------------------------------------------------------//