This program supports the Image processing micon car production kit (M-S348).
Dependencies: GR-PEACH_video mbed
main.cpp
- Committer:
- TetsuyaKonno
- Date:
- 2018-10-30
- Revision:
- 0:00b6f7454ada
File content as of revision 0:00b6f7454ada:
//------------------------------------------------------------------// //Supported MCU: RZ/A1H //File Contents: kit18 GR-peach ( Trace program ) //Version number: Ver.1.00 //Date: 2018.10.30 //Copyright: Renesas Electronics Corporation // Hitachi Document Solutions Co., Ltd. //------------------------------------------------------------------// //This program supports the following kit: //* M-S348 Image processing micon car production kit //------------------------------------------------------------------// //Include //------------------------------------------------------------------// #include "mbed.h" #include "iodefine.h" #include "DisplayBace.h" #include "image_process.h" //------------------------------------------------------------------// //Define //------------------------------------------------------------------// //Motor PWM cycle #define MOTOR_PWM_CYCLE 33332 /* Motor PWM period */ /* 1ms P0φ/1 = 0.03us */ //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 */ #define THRESHOLD 128 /* Binarization function only */ //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 //led_m_set function only #define RUN 0x00 #define STOP 0x01 #define ERROR 0x02 #define DEBUG 0x03 #define CRANK 0x04 #define LCHANGE 0x05 //ImageData_Serial_out3 function only #define COLOR 0x01 #define GREY_SCALE 0x02 #define BINARY 0x03 //------------------------------------------------------------------// //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 (160u) /* QVGA */ #define PIXEL_VW (120u) /* QVGA */ #define VIDEO_BUFFER_STRIDE (((PIXEL_HW * DATA_SIZE_PER_PIC) + 31u) & ~31u) #define VIDEO_BUFFER_HEIGHT (PIXEL_VW) //------------------------------------------------------------------// //Constructor //------------------------------------------------------------------// /* Create DisplayBase object */ DisplayBase Display; 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 */ BusIn dipsw( P7_15, P8_1, P2_9, P2_10 ); /* SW1 on Shield board */ DigitalOut Left_motor_signal(P4_6); /* Used by motor function */ DigitalOut Right_motor_signal(P4_7); /* Used by motor function */ 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) //------------------------------------------------------------------// void init_Camera( void ); void ChangeFrameBuffer( void ); 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 */ //Interrupt function void intTimer( void ); /* 1ms period */ //GR-peach board void led_rgb(int led); void led_m_user( int led ); unsigned char user_button_get( void ); void led_m_set( int set ); void led_m_process( void ); /* Only function for interrupt */ //Motor drive board void led_out(int led); unsigned char pushsw_get( void ); void motor( int accele_l, int accele_r ); void motor2( int accele_l, int accele_r ); void handle( int angle ); int diff( int pwm ); //Shield board unsigned char dipsw_get( void ); //------------------------------------------------------------------// //Prototype( Digital sensor process ) //------------------------------------------------------------------// int init_sensor( unsigned char *BuffAddrIn, int HW, int VW, int Cx, int *SENPx, int Y ); unsigned char sensor_process( unsigned char *BuffAddrIn, int HW, int VW, int *SENPx, int Y ); /* Only function for interrupt */ unsigned char sensor_inp( void ); unsigned char center_inp( void ); //------------------------------------------------------------------// //Prototype( Mark detect functions ) //------------------------------------------------------------------// int RightCrankCheck( int percentage ); int RightLaneChangeCheck( int percentage ); //------------------------------------------------------------------// //Prototype( Debug functions ) //------------------------------------------------------------------// void ImageData_Serial_Out1( unsigned char *ImageData, int HW, int VW ); void ImageData_Serial_Out2( unsigned char *ImageData, int HW, int VW ); void ImageData_Serial_Out3( unsigned char *ImageData, int HW, int VW, int color_pattern ); //------------------------------------------------------------------// //Global variable (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!; uint8_t * write_buff_addr = FrameBuffer_Video_A; uint8_t * save_buff_addr = FrameBuffer_Video_B; 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; //------------------------------------------------------------------// //Global variable for Image process //------------------------------------------------------------------// unsigned char ImageData_A[ ( ( PIXEL_HW * 2) * PIXEL_VW ) ]; unsigned char ImageData_B[ ( PIXEL_HW * PIXEL_VW ) ]; unsigned char ImageComp_B[ ( PIXEL_HW * PIXEL_VW ) ]; unsigned char ImageBinary[ ( PIXEL_HW * PIXEL_VW ) ]; double Rate = 0.125; /* Reduction ratio */ //------------------------------------------------------------------// //Global variable for Digital sensor function //------------------------------------------------------------------// int SenError; int Sen1Px[5]; unsigned char SenVal1; //------------------------------------------------------------------// //Global variable for Mark detection function //------------------------------------------------------------------// ImagePartPattern RightCrank = {0,0,0,0,{0}, //percent, Point X, Point Y, Standard_Deviation, Deviation {0,0,0,0,0,0,0, //Binary image 0,0,1,1,1,1,1, //Binary image 0,0,1,1,1,0,0, //Binary image 0,0,1,1,1,0,0},//Binary image 7, 4}; //Binary Width pixel, Binary Height pixel ImagePartPattern RightLaneChange = {0,0,0,0,{0},//percent, Point X, Point Y, Standard_Deviation, Deviation {0,0,0,1,1,0,0,0,0,0,0,0,0,0, //Binary image 0,0,0,1,1,0,0,0,0,0,0,0,0,0, //Binary image 0,0,0,1,1,0,0,0,0,0,0,0,0,0},//Binary image 14, 3}; //Binary Width pixel, Binary Height pixel //------------------------------------------------------------------// //Global 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 led_pattern; /* led_m_process function only */ volatile int initFlag; /* Initialize flag */ volatile int threshold_buff; /* Binarization function only */ volatile int handle_buff; /* diff function only */ const int revolution_difference[] = { /* diff function only */ 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 ) { volatile int Number; /* Serial Debug Mode only */ initFlag = 1; /* Initialization start */ /* Camera start */ init_Camera(); /* wait to stabilize NTSC signal (about 170ms) */ wait(0.2); /* Initialize MCU functions */ init_MTU2_PWM_Motor(); init_MTU2_PWM_Servo(); interrput.attach(&intTimer, 0.001); pc.baud(230400); /* Initialize Micon Car state */ handle( 0 ); motor( 0, 0 ); led_out( 0x0 ); led_m_set( STOP ); threshold_buff = THRESHOLD; wait(0.5); /* Initialize Digital sensor */ SenError = init_sensor( ImageBinary, (PIXEL_HW * Rate), (PIXEL_VW * Rate), (PIXEL_HW * Rate) / 2, Sen1Px, 12 ); if( SenError !=0 ) { led_m_set( ERROR ); cnt1 = 0; while( cnt1 < 3000 ){ if( cnt1 % 200 < 100 ) { led_out( 0x3 ); } else { led_out( 0x0 ); } } } /* Initialize Pattern Matching */ RightCrank.sdevi = Standard_Deviation( RightCrank.binary, RightCrank.devi, RightCrank.w, RightCrank.h ); RightLaneChange.sdevi = Standard_Deviation( RightLaneChange.binary, RightLaneChange.devi, RightLaneChange.w, RightLaneChange.h ); initFlag = 0; /* Initialization end */ /* Debug Program */ if( user_button_get() ) { led_m_set( DEBUG ); wait(0.1);//ON Time while( user_button_get() ); wait(0.5);//OFF Time while( 1 ){ pc.printf( "Serial Debug Mode Ver1.0 (2018.10.30)\n\r" ); pc.printf( "\n\r" ); pc.printf( "0:TeraTram Real-time display 20* 15pixel (Binary)\n\r" ); pc.printf( "1:Excel(csv) 160*120pixel\n\r" ); pc.printf( "2:Excel(csv) 20* 15pixel\n\r" ); pc.printf( "3:Excel(csv) 160*120pixel -> csv_jpg_convert.bat (color)\n\r" ); pc.printf( "4:Excel(csv) 160*120pixel -> csv_jpg_convert.bat (grey scale)\n\r" ); pc.printf( "5:Excel(csv) 20* 15pixel -> csv_jpg_convert.bat (binary)\n\r" ); pc.printf( "\n\r" ); pc.printf( "Please Number\n\r" ); pc.printf( "No = " ); pc.scanf( "%d", &Number ); pc.printf( "\n\r" ); pc.printf( "Please push the SW ( on the Motor drive board )\n\r" ); pc.printf( "\n\r" ); while( !pushsw_get() ); switch( Number ) { case 0: /* for TeraTerm(Real-time display) */ while( 1 ) { ImageData_Serial_Out2( ImageBinary, (PIXEL_HW * Rate), (PIXEL_VW * Rate) ); } break; case 1: /* for the Excel(csv) 160*120pixel */ ImageData_Serial_Out1( ImageData_B, PIXEL_HW, PIXEL_VW ); break; case 2: /* for the Excel(csv) 20* 15pixel */ ImageData_Serial_Out1( ImageComp_B, (PIXEL_HW * Rate), (PIXEL_VW * Rate) ); break; case 3: /* for the Excel(csv) -> csv_jpg_convert.bat */ ChangeFrameBuffer(); ImageData_Serial_Out3( save_buff_addr, PIXEL_HW, PIXEL_VW, COLOR ); break; case 4: /* for the Excel(csv) -> csv_jpg_convert.bat */ ImageData_Serial_Out3( ImageData_B, PIXEL_HW, PIXEL_VW, GREY_SCALE ); break; case 5: /* for the Excel(csv) -> csv_jpg_convert.bat */ ImageData_Serial_Out3( ImageBinary, (PIXEL_HW * Rate), (PIXEL_VW * Rate), BINARY ); break; default: break; } led_m_set( STOP ); while(1); } } /* Trace program */ 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( RightCrankCheck( 85 ) ) { /* Right Crank Check */ pattern = 31; break; } if( RightLaneChangeCheck( 85 ) ) { /* Right Lane Change Check */ pattern = 51; break; } switch( (sensor_inp()&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( (sensor_inp()&0x02) == 0x02 ) { pattern = 11; break; } switch( (sensor_inp()&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( (sensor_inp()&0x04) == 0x04 ) { pattern = 11; } switch( (sensor_inp()&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 31: /* Right crank processing at 1st process */ led_m_set( CRANK ); led_out( 0x1 ); handle( 0 ); motor2( 0 ,0 ); pattern = 32; cnt1 = 0; break; case 32: /* Right crank processing at 2nd process */ if( cnt1 > 300 ) { pattern = 33; cnt1 = 0; } break; case 33: /* Right crank processing at 3rd process */ if( ( center_inp() == 0 ) && ( (sensor_inp()&0x0f) == 0x00 ) ) { handle( 41 ); motor2( 50 ,10 ); pattern = 34; cnt1 = 0; break; } switch( (sensor_inp()&0x0f) ) { case 0x00: /* Center -> straight */ handle( 0 ); motor2( 30 ,30 ); break; case 0x02: /* Left of center -> turn to right */ handle( 5 ); motor2( 30 ,diff(30) ); break; case 0x04: /* Right of center -> turn to left */ handle( -5 ); motor2( diff(30) ,30 ); break; default: break; } break; case 34: /* Right crank clearing processing - wait until stable */ if( cnt1 > 800 ) { pattern = 35; cnt1 = 0; } break; case 35: /* Right crank clearing processing - check end of turn */ if( (sensor_inp()&0x02) == 0x02 ) { led_out( 0x0 ); led_m_set( RUN ); pattern = 11; cnt1 = 0; } break; case 51: /* Right lane change processing at 1st process */ led_m_set( LCHANGE ); led_out( 0x01 ); handle( 0 ); motor2( 0 ,0 ); pattern = 52; cnt1 = 0; break; case 52: /* Right lane change processing at 2nd process */ if( cnt1 > 300 ) { pattern = 53; cnt1 = 0; break; } break; case 53: /* Right lane change processing at 3rd process */ if( ( center_inp() == 0 ) && ( (sensor_inp()&0x0f) == 0x00 ) ) { handle( 20 ); motor2( 30 ,diff(30) ); pattern = 54; cnt1 = 0; break; } switch( (sensor_inp()&0x0f) ) { case 0x00: /* Center -> straight */ handle( 0 ); motor2( 30 ,30 ); break; case 0x02: /* Left of center -> turn to right */ handle( 5 ); motor2( 30 ,diff(30) ); break; case 0x04: /* Right of center -> turn to left */ handle( -5 ); motor2( diff(30) ,30 ); break; default: break; } break; case 54: /* Right lane change clearing processing - wait until stable */ if( cnt1 > 800 ){ pattern = 55; cnt1 = 0; } break; case 55: /* Right lane change end check */ if( (sensor_inp()&0x04) == 0x04 ) { led_m_set( RUN ); led_out( 0x0 ); pattern = 11; cnt1 = 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 Control */ /* 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 */ /* Module 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 Control */ /* 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 */ /* Module 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 */ } //------------------------------------------------------------------// //Initialize Camera function //------------------------------------------------------------------// void init_Camera( void ) { /* NTSC-Video */ DisplayBase::graphics_error_t error; /* Graphics initialization process */ error = Display.Graphics_init(NULL); if (error != DisplayBase::GRAPHICS_OK) { pc.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) { pc.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) { pc.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) { pc.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) { pc.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) { pc.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) { pc.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); } //------------------------------------------------------------------// //ChangeFrameBuffer function //------------------------------------------------------------------// void ChangeFrameBuffer( void ) { /* NTSC-Video */ DisplayBase::graphics_error_t error; /* Change write 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; } error = Display.Video_Write_Change( VIDEO_INPUT_CH, write_buff_addr, VIDEO_BUFFER_STRIDE); if (error != DisplayBase::GRAPHICS_OK) { pc.printf("Line %d, error %d\n", __LINE__, error); while (1); } } //------------------------------------------------------------------// // @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 vfield_count2 = 0; led_m_user( vfield_count2 ); } //------------------------------------------------------------------// // @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; /* Only variable for image process */ cnt0++; cnt1++; /* field check */ if( vfield_count2 != vfield_count2_buff ) { vfield_count2_buff = vfield_count2; counter = 0; } /* Top field / bottom field */ switch( counter++ ) { case 0: ImageCopy( write_buff_addr, PIXEL_HW, PIXEL_VW, ImageData_A, vfield_count2 ); break; case 1: ImageCopy( write_buff_addr, PIXEL_HW, PIXEL_VW, ImageData_A, vfield_count2 ); break; case 2: Extraction_Brightness( ImageData_A, PIXEL_HW, PIXEL_VW, ImageData_B, vfield_count2 ); break; case 3: Extraction_Brightness( ImageData_A, PIXEL_HW, PIXEL_VW, ImageData_B, vfield_count2 ); break; case 4: ImageReduction( ImageData_B, PIXEL_HW, PIXEL_VW, ImageComp_B, Rate ); break; case 5: ImageReduction( ImageData_B, PIXEL_HW, PIXEL_VW, ImageComp_B, Rate ); break; case 6: Binarization( ImageComp_B, (PIXEL_HW * Rate), (PIXEL_VW * Rate), ImageBinary, threshold_buff ); if( !initFlag ) SenVal1 = sensor_process( ImageBinary, (PIXEL_HW * Rate), (PIXEL_VW * Rate), Sen1Px, 12 ); break; case 7: if( !initFlag ) PatternMatching_process( ImageBinary, (PIXEL_HW * Rate), (PIXEL_VW * Rate), &RightCrank, 2, 10, 2, 8 ); break; case 8: if( !initFlag ) PatternMatching_process( ImageBinary, (PIXEL_HW * Rate), (PIXEL_VW * Rate), &RightLaneChange, 4, 6, 1, 3 ); break; default: break; } /* LED(rgb) on the GR-peach board */ 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 char 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; } //------------------------------------------------------------------// //led_m_set Function //------------------------------------------------------------------// void led_m_set( int set ) { led_pattern = set; } //------------------------------------------------------------------// //led_m_process Function for only interrupt //------------------------------------------------------------------// void led_m_process( void ) { int led_color; int onTime; int offTime; static unsigned long cnt_led_m; switch( led_pattern ) { case RUN: led_color = LED_GREEN; onTime = 500; offTime = 500; break; case STOP: led_color = LED_RED; onTime = 500; offTime = 0; break; case ERROR: led_color = LED_RED; onTime = 100; offTime = 100; break; case DEBUG: led_color = LED_BLUE; onTime = 50; offTime = 50; break; case CRANK: led_color = LED_YELLOW; onTime = 500; offTime = 500; break; case LCHANGE: led_color = LED_BLUE; onTime = 500; offTime = 500; break; default: led_color = LED_OFF; onTime = 500; offTime = 500; break; } cnt_led_m++; /* Display */ if( cnt_led_m < onTime ) led_rgb( led_color ); else if( cnt_led_m < ( onTime + offTime ) ) led_rgb( LED_OFF ); else cnt_led_m = 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 char 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 ) { int sw_data; sw_data = dipsw_get() + 5; accele_l = ( accele_l * sw_data ) / 20; accele_r = ( accele_r * sw_data ) / 20; /* 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; } } //------------------------------------------------------------------// //motor speed control(PWM) //Arguments: motor:-100 to 100 //Here, 0 is stop, 100 is forward, -100 is reverse //------------------------------------------------------------------// void motor2( int accele_l, int accele_r ) { /* 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; } //******************************************************************// // functions ( on Shield board ) //*******************************************************************/ //------------------------------------------------------------------// //Dipsw get Function //------------------------------------------------------------------// unsigned char dipsw_get( void ) { return( dipsw.read() & 0x0f ); } //******************************************************************// // digital sensor functions //*******************************************************************/ //------------------------------------------------------------------// // init sensor Function //------------------------------------------------------------------// int init_sensor( unsigned char *BuffAddrIn, int HW, int VW, int Cx, int *SENPx, int Y ) { int X; int L1x, R1x; int i, error_cnt; //Left side for( X = Cx; X > 1; X-- ){ if( BuffAddrIn[ ( Y * HW ) + ( X - 0 ) ] == 0 && BuffAddrIn[ ( Y * HW ) + ( X - 1 ) ] == 0 ) { L1x = X; break; } } //Right side for( X = Cx; X < ( HW - 1 ); X++ ){ if( BuffAddrIn[ ( Y * HW ) + ( X + 0 ) ] == 0 && BuffAddrIn[ ( Y * HW ) + ( X + 1 ) ] == 0 ) { R1x = X; break; } } SENPx[ 4 ] = L1x + ( ( R1x - L1x ) / 2 ); // Center SENPx[ 2 ] = L1x; // L1 SENPx[ 1 ] = R1x; // R1 SENPx[ 3 ] = SENPx[ 2 ] - ( SENPx[ 4 ] - SENPx[ 2 ] ); //L2 SENPx[ 0 ] = SENPx[ 1 ] + ( SENPx[ 1 ] - SENPx[ 4 ] ); //R2 /* error check */ error_cnt = 0; for( i = 0; i < 4; i++ ){ if( SENPx[4] == SENPx[i] ) error_cnt += 1; } //#define DEBUG_MODE #ifdef DEBUG_MODE pc.printf( "L2=%2d, L1=%2d, Cx=%2d, R1=%2d, R2=%2d\n\r", SENPx[ 3 ], SENPx[ 2 ], SENPx[ 4 ], SENPx[ 1 ], SENPx[ 0 ] ); if( error_cnt != 0 ) pc.printf( "init_sensor function Error %1d\n\r", error_cnt ); else pc.printf( "init_sensor function complete\n\r" ); pc.printf( "\n\r" ); #endif return error_cnt; } //------------------------------------------------------------------// //sensor_process Function(Interrupt function) //------------------------------------------------------------------// unsigned char sensor_process( unsigned char *BuffAddrIn, int HW, int VW, int *SENPx, int Y ) { int sensor; int data; sensor = 0x00; if( VW < Y ) { pc.printf( "sensor process function error\n\r" ); return sensor; } data = BuffAddrIn[ ( Y * HW ) + SENPx[ 4 ] ] & 0x01; sensor |= ( data << 4 ) & 0x10; data = BuffAddrIn[ ( Y * HW ) + SENPx[ 3 ] ] & 0x01; sensor |= ( data << 3 ) & 0x08; data = BuffAddrIn[ ( Y * HW ) + SENPx[ 2 ] ] & 0x01; sensor |= ( data << 2 ) & 0x04; data = BuffAddrIn[ ( Y * HW ) + SENPx[ 1 ] ] & 0x01; sensor |= ( data << 1 ) & 0x02; sensor |= BuffAddrIn[ ( Y * HW ) + SENPx[ 0 ] ] & 0x01; sensor &= 0x1f; return sensor; } //------------------------------------------------------------------// //sensor Function //------------------------------------------------------------------// unsigned char sensor_inp( void ) { return SenVal1 & 0x0f; } //------------------------------------------------------------------// //sensor Function //------------------------------------------------------------------// unsigned char center_inp( void ) { return ( SenVal1 >> 4 ) & 0x01; } //------------------------------------------------------------------// // RightCrank Check // Return values: 0: no triangle mark, 1: Triangle mark //------------------------------------------------------------------// int RightCrankCheck( int percentage ) { int ret; ret = 0; if( RightCrank.p >= percentage ) { ret = 1; } return ret; } //------------------------------------------------------------------// // RightLaneChange Check // Return values: 0: no triangle mark, 1: Triangle mark //------------------------------------------------------------------// int RightLaneChangeCheck( int percentage ) { int ret; ret = 0; if( RightLaneChange.p >= percentage ) { ret = 1; } return ret; } //******************************************************************// // Debug functions //*******************************************************************/ //------------------------------------------------------------------// //Image Data Output( for the Excel ) //------------------------------------------------------------------// void ImageData_Serial_Out1( unsigned char *ImageData, int HW, int VW ) { int Xp, Yp, inc; for( Yp = 0, inc = 0; Yp < VW; Yp++ ) { for( Xp = 0; Xp < HW; Xp++, inc++ ) { pc.printf( "%d,", ImageData[ inc ] ); } pc.printf("\n\r"); } } //------------------------------------------------------------------// //Image Data Output2( for TeraTerm ) //------------------------------------------------------------------// void ImageData_Serial_Out2( unsigned char *ImageData, int HW, int VW ) { int Xp, Yp; int i; for( Yp = 0; Yp < VW; Yp++ ) { for( Xp = 0; Xp < HW; Xp++ ) { pc.printf( "%d ", ImageData[Xp + (Yp * HW)] ); } pc.printf( "\n\r" ); } //Add display pc.printf( "\n\r" ); pc.printf( "sensor_inp = 0x%02x\n\r", sensor_inp() ); pc.printf( "center_inp = 0x%02x\n\r", center_inp() ); pc.printf( "RightCrank = %01d, = %3d%%, X = %2d, Y = %2d\n\r", RightCrankCheck(80), RightCrank.p, RightCrank.x, RightCrank.y ); pc.printf( "RightLaneChange = %01d, = %3d%%, X = %2d, Y = %2d\n\r", RightLaneChangeCheck(80), RightLaneChange.p, RightLaneChange.x, RightLaneChange.y ); pc.printf( "\n\r" ); VW += 6; pc.printf( "Threshold Check ( please user button on GR-peach board )\n\r" ); pc.printf( "\n\r" ); VW += 2; if( user_button_get() ){ pc.printf( "Please Threshold\n\r" ); pc.printf( " = " ); pc.scanf( "%d", &i ); pc.printf( "\n\r" ); threshold_buff = i; VW += 2; } pc.printf( "\033[%dA" , VW ); } //------------------------------------------------------------------// //Image Data Output3( for TeraTerm ) //------------------------------------------------------------------// void ImageData_Serial_Out3( unsigned char *ImageData, int HW, int VW, int color_pattern ) { int X, Y; int Px, Py; int HW_T;//HW Twice int value; HW_T = HW + HW; /* Camera module test process 2 */ pc.printf( "//,X-Size,Y-Size" ); pc.printf( "\n\r" ); pc.printf( "#SIZE,%3d,%3d", HW, VW ); pc.printf( "\n\r" ); pc.printf( "//,X-Point,Y-Point" ); pc.printf( "\n\r" ); switch( color_pattern ) { case COLOR: //YCBCR_422 Color for( Py = 0, Y = 0; Py < VW; Py+=1, Y++ ){ for( Px = 0, X = 0; Px < HW_T; Px+=4, X+=2 ){ pc.printf( "#YCbCr," ); /*Xp*/pc.printf( "%d,", X); /*Yp*/pc.printf( "%d,", Y); /*Y0*/pc.printf( "%d,", ImageData[ ( Px + 0 ) + ( HW_T * Py ) ] );//6 /*Cb*/pc.printf( "%d,", ImageData[ ( Px + 1 ) + ( HW_T * Py ) ] );//5 /*Cr*/pc.printf( "%d,", ImageData[ ( Px + 3 ) + ( HW_T * Py ) ] );//7 pc.printf( "\n\r" ); pc.printf( "#YCbCr," ); /*Xp*/pc.printf( "%d,", X+1); /*Yp*/pc.printf( "%d,", Y); /*Y1*/pc.printf( "%d,", ImageData[ ( Px + 2 ) + ( HW_T * Py ) ] );//4 /*Cb*/pc.printf( "%d,", ImageData[ ( Px + 1 ) + ( HW_T * Py ) ] );//5 /*Cr*/pc.printf( "%d,", ImageData[ ( Px + 3 ) + ( HW_T * Py ) ] );//7 pc.printf( "\n\r" ); } } break; case GREY_SCALE: //YCBCR_422 GreyScale for( Y = 0; Y < VW; Y++ ){ for( X = 0; X < HW; X+=2 ){ pc.printf( "#YCbCr," ); /*Xp*/pc.printf( "%d,", X); /*Yp*/pc.printf( "%d,", Y); /*Y0*/pc.printf( "%d,", ImageData[ ( Y * HW ) + ( X + 0 ) ] );//6 /*Cb*/pc.printf( "%d,", 128);//5 /*Cr*/pc.printf( "%d,", 128);//7 pc.printf( "\n\r" ); pc.printf( "#YCbCr," ); /*Xp*/pc.printf( "%d,", X+1); /*Yp*/pc.printf( "%d,", Y); /*Y1*/pc.printf( "%d,", ImageData[ ( Y * HW ) + ( X + 1 ) ] );//4 /*Cb*/pc.printf( "%d,", 128);//5 /*Cr*/pc.printf( "%d,", 128);//7 pc.printf( "\n\r" ); } } break; case BINARY: //YCBCR_422 Binary for( Y = 0; Y < VW; Y++ ){ for( X = 0; X < HW; X+=2 ){ pc.printf( "#YCbCr," ); /*Xp*/pc.printf( "%d,", X); /*Yp*/pc.printf( "%d,", Y); value = ImageData[ ( Y * HW ) + ( X + 0 ) ]; if( value ) value = 255; else value = 0; /*Y0*/pc.printf( "%d,", value );//6 /*Cb*/pc.printf( "%d,", 128);//5 /*Cr*/pc.printf( "%d,", 128);//7 pc.printf( "\n\r" ); pc.printf( "#YCbCr," ); /*Xp*/pc.printf( "%d,", X+1); /*Yp*/pc.printf( "%d,", Y); value = ImageData[ ( Y * HW ) + ( X + 1 ) ]; if( value ) value = 255; else value = 0; /*Y1*/pc.printf( "%d,", value );//4 /*Cb*/pc.printf( "%d,", 128);//5 /*Cr*/pc.printf( "%d,", 128);//7 pc.printf( "\n\r" ); } } break; default: break; } pc.printf( "End\n\r" ); } //------------------------------------------------------------------// // End of file //------------------------------------------------------------------//