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