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