![](/media/cache/profiles/9bb89ae4cdafb337ee8ddcc52277bf05.jpg.50x50_q85.png)
Forked from upstream.
Dependencies: GR-PEACH_video mbed
Fork of TraceMark_Program_60fps by
main.cpp
- Committer:
- osaboh
- Date:
- 2017-01-14
- Revision:
- 2:dd32c5987b14
- Parent:
- 1:4a94c87a7d04
File content as of revision 2:dd32c5987b14:
//------------------------------------------------------------------// //Supported MCU: RZ/A1H //File Contents: Trace Program by Image Processing // (GR-PEACH version on the Micon Car) //Version number: Ver.1.01 //Date: 2017.01.10 //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; break; } 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; counter = 0; } /* Top field / bottom field */ led_m_user( vfield_count2 ); 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; 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; // !!!! not initilazed !!!! 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 //------------------------------------------------------------------//