Micon Car Rally
Added program for mark detection (Cross line, Left half line and Right half line).
Dependencies: GR-PEACH_video mbed
Fork of
TraceMark_Program_60fps
by 
Micon Car Rally
Diff: main.cpp
- Revision:
- 2:af4db183ee76
- Parent:
- 1:4a94c87a7d04
--- a/main.cpp Tue Jan 10 04:59:48 2017 +0000
+++ b/main.cpp Fri Mar 17 03:53:01 2017 +0000
@@ -2,17 +2,17 @@
//Supported MCU: RZ/A1H
//File Contents: Trace Program by Image Processing
// (GR-PEACH version on the Micon Car)
-//Version number: Ver.1.01
-//Date: 2017.01.10
+//Version number: Ver.2.00
+//Date: 2017.03.17
//Copyright: Renesas Electronics Corporation
// Hitachi Document Solutions Co., Ltd.
//------------------------------------------------------------------//
-
+
//This program supports the following boards:
//* GR-PEACH(E version)
//* Motor drive board Ver.5
//* Camera module (SC-310)
-
+
//Include
//------------------------------------------------------------------//
#include "mbed.h"
@@ -52,6 +52,9 @@
#define MOTOR_START 0x04
#define MOTOR_STOP 0x05
#define MARK_T 0x06
+#define MARK_C 0x07
+#define MARK_R 0x08
+#define MARK_L 0x09
//Define(NTSC-Video)
//------------------------------------------------------------------//
@@ -107,6 +110,7 @@
void led_out(int led);
unsigned int 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 );
@@ -128,8 +132,16 @@
double Standard_Deviation( unsigned char *data, double *Devi, int items );
double Covariance( double *Devi_A, double *Devi_B, int items );
int Judgement_ImageMatching( double covari, double SDevi_A, double SDevi_B );
+
void MarkDetect_process_T( void );
+void MarkDetect_process_C( void );
+void MarkDetect_process_R( void );
+void MarkDetect_process_L( void );
+
int MarkCheck_Triangle( int percentage );
+int MarkCheck_Crossline( int percentage );
+int MarkCheck_Rightline( int percentage );
+int MarkCheck_Leftline( int percentage );
//Prototype(Display Debug)
//------------------------------------------------------------------//
@@ -162,18 +174,32 @@
unsigned char TempBinary_Triangle[15] = {0,1,1,1,0,
0,0,1,0,0,
0,0,0,0,0};
-
-double NowDevi[15];
-unsigned char NowImageBinary[15];
+double TempDevi_Crossline[28];
+unsigned char TempBinary_Crossline[28] = {0,0,0,0,0,1,1,1,1,0,0,0,0,0,
+ 1,1,1,1,1,1,1,1,1,1,1,1,1,1};
+double TempDevi_Rightline[28];
+unsigned char TempBinary_Rightline[28] = {0,0,0,0,0,1,1,1,1,0,0,0,0,0,
+ 0,0,0,0,0,0,1,1,1,1,1,1,1,1};
+double TempDevi_Leftline[28];
+unsigned char TempBinary_Leftline[28] = {0,0,0,0,0,1,1,1,1,0,0,0,0,0,
+ 1,1,1,1,1,1,1,1,0,0,0,0,0,0};
+double NowDevi[30];
+unsigned char NowImageBinary[30];
volatile double retDevi_Triangle;
+volatile double retDevi_Crossline;
+volatile double retDevi_Rightline;
+volatile double retDevi_Leftline;
volatile double retDevi;
volatile double retCovari;
volatile int retJudgeIM;
-volatile int retJudgeIM_Max[1];
+volatile int retJudgeIM_Max[4];
int Xt, Yt;
+int Xc, Yc;
+int Xr, Yr;
+int Xl, Yl;
//Globle variable for led fanction
//------------------------------------------------------------------//
@@ -186,7 +212,10 @@
LED_BLUE, 50, 50, /* DEBUG */
LED_GREEN, 1, 0, /* MOTOR_START */
LED_RED, 1, 0, /* MOTOR_STOP */
- LED_WHITE, 500, 500}; /* MARK_T */
+ LED_WHITE, 500, 500, /* MARK_T */
+ LED_YELLOW, 500, 500, /* MARK_C */
+ LED_PURPLE, 500, 500, /* MARK_R */
+ LED_SKYBLUE,500, 500}; /* MARK_L */
//Globle variable for Trace program
//------------------------------------------------------------------//
@@ -195,6 +224,8 @@
volatile int pattern; /* Pattern numbers */
volatile int handle_buff;
+volatile unsigned long main_cnt = 0; /* Used by timer function */
+
const int revolution_difference[] = {
100, 98, 97, 95, 93,
92, 90, 88, 87, 85,
@@ -306,6 +337,9 @@
/* Initialize Mark detection */
retDevi_Triangle = Standard_Deviation( TempBinary_Triangle, TempDevi_Triangle, 15 );
+ retDevi_Crossline = Standard_Deviation( TempBinary_Crossline, TempDevi_Crossline, 28 );
+ retDevi_Rightline = Standard_Deviation( TempBinary_Rightline, TempDevi_Rightline, 28 );
+ retDevi_Leftline = Standard_Deviation( TempBinary_Leftline, TempDevi_Leftline, 28 );
if( user_button_get() ) {
wait(0.1);
@@ -314,16 +348,15 @@
wait(0.5);
pc.printf( "Please push the SW ( on the Motor drive board )\n\r" );
pc.printf( "\n\r" );
- while( !user_button_get() );
+ while( !pushsw_get() );
while( 1 ){
ImageData_Serial_Out2( ImageBinary, 20 );
}
}
-
led_m_set( RUN );
while( 1 ) {
-
+
switch( pattern ) {
/*****************************************************************
About patern
@@ -350,7 +383,7 @@
cnt1 = 0;
}
break;
-
+
case 11:
/* normal trace */
if( MarkCheck_Triangle( 90 ) ) {
@@ -358,6 +391,21 @@
pattern = 91;
break;
}
+ if( MarkCheck_Crossline( 90 ) ) { /* Cross line check */
+ led_m_set( MARK_C );
+ pattern = 21;
+ break;
+ }
+ if( MarkCheck_Rightline( 90 ) ) { /* Right half line detection check */
+ led_m_set( MARK_R );
+ pattern = 51;
+ break;
+ }
+ if( MarkCheck_Leftline( 90 ) ) { /* Left half line detection check */
+ led_m_set( MARK_L );
+ pattern = 61;
+ break;
+ }
switch( (digital_sensor()&0x0f) ) {
case 0x00:
handle( 0 );
@@ -393,7 +441,7 @@
break;
}
break;
-
+
case 12:
/* Left side */
if( (digital_sensor()&0x02) == 0x02 ) {
@@ -415,7 +463,7 @@
break;
}
break;
-
+
case 13:
/* right side */
if( (digital_sensor()&0x04) == 0x04 ) {
@@ -437,8 +485,255 @@
}
break;
+ case 21:
+ /* Processing at 1st cross line */
+ led_out( 0x3 );
+ handle( 0 );
+ motor2( 0 ,0 );
+ pattern = 22;
+ cnt1 = 0;
+ break;
+
+ case 22:
+ /* Read but ignore 2nd line */
+ if( cnt1 > 100 ) {
+ pattern = 23;
+ cnt1 = 0;
+ }
+ break;
+
+ case 23:
+ /* Trace, crank detection after cross line */
+ if( (digital_sensor()&0x0c) == 0x0c) {
+ /* Left crank determined -> to left crank clearing processing */
+ led_out( 0x1 );
+ pattern = 31;
+ cnt1 = 0;
+ break;
+ }
+ if( (digital_sensor()&0x03) == 0x03 ) {
+ /* Right crank determined -> to right crank clearing processing */
+ led_out( 0x2 );
+ pattern = 41;
+ cnt1 = 0;
+ break;
+ }
+ switch( (digital_sensor()&0x0f) ) {
+ case 0x00:
+ /* Center -> straight */
+ handle( 0 );
+ motor2( 27 ,27 );
+ break;
+ case 0x02:
+ /* Left of center -> turn to right */
+ handle( 4 );
+ motor2( 27 ,diff(27) );
+ break;
+ case 0x04:
+ /* Right of center -> turn to left */
+ handle( -4 );
+ motor2( diff(27) ,27 );
+ break;
+ default:
+ break;
+ }
+ break;
+
+ case 31:
+ if( (digital_sensor()&0x0f) == 0x00 ) {
+ handle( -40 );
+ motor2( diff(25) ,45 );
+ pattern = 32;
+ cnt1 = 0;
+ }
+ break;
+
+ case 32:
+ /* Left crank clearing processing - wait until stable */
+ if( cnt1 > 300 ) {
+ pattern = 33;
+ cnt1 = 0;
+ }
+ break;
+
+ case 33:
+ /* Left crank clearing processing - check end of turn */
+ if( (digital_sensor()&0x04) == 0x04 ) {
+ led_out( 0x0 );
+ led_m_set( RUN );
+ pattern = 11;
+ cnt1 = 0;
+ }
+ break;
+
+ case 41:
+ if( (digital_sensor()&0x0f) == 0x00 ) {
+ handle( 41 );
+ motor2( 50 ,diff(25) );
+ pattern = 42;
+ cnt1 = 0;
+ }
+ break;
+
+ case 42:
+ /* Right crank clearing processing - wait until stable */
+ if( cnt1 > 300 ) {
+ pattern = 43;
+ cnt1 = 0;
+ }
+ break;
+
+ case 43:
+ /* Right crank clearing processing - check end of turn */
+ if( (digital_sensor()&0x02) == 0x02 ) {
+ led_out( 0x0 );
+ led_m_set( RUN );
+ pattern = 11;
+ cnt1 = 0;
+ }
+ break;
+
+ case 51:
+ /* Processing at 1st right half line detection */
+ led_out( 0x2 );
+ handle( 0 );
+ motor2( 0 ,0 );
+ pattern = 52;
+ cnt1 = 0;
+ break;
+
+ case 52:
+ /* Read but ignore 2nd time */
+ if( cnt1 > 100 ) {
+ pattern = 53;
+ cnt1 = 0;
+ break;
+ }
+ if( MarkCheck_Crossline( 90 ) ) { /* Cross line check */
+ led_m_set( MARK_C );
+ pattern = 21;
+ break;
+ }
+ break;
+
+ case 53:
+ /* Trace, lane change after right half line detection */
+ if( (digital_sensor()&0x1f) == 0x00 ) {
+ handle( 22 );
+ motor2( 35 ,diff(25) );
+ pattern = 54;
+ cnt1 = 0;
+ break;
+ }
+ switch( (digital_sensor()&0x0f) ) {
+ case 0x00:
+ /* Center -> straight */
+ handle( 0 );
+ motor2( 30 ,30 );
+ break;
+ case 0x02:
+ /* Left of center -> turn to right */
+ handle( 4 );
+ motor2( 30 ,diff(30) );
+ break;
+ case 0x04:
+ /* Right of center -> turn to left */
+ handle( -4 );
+ motor2( diff(30) ,30 );
+ break;
+ default:
+ break;
+ }
+ break;
+
+ case 54:
+ if( cnt1 > 500 ){
+ pattern = 55;
+ cnt1 = 0;
+ }
+ break;
+
+ case 55:
+ /* Right lane change end check */
+ if( (digital_sensor()&0x04) == 0x04 ) {
+ led_out( 0x0 );
+ led_m_set( RUN );
+ pattern = 11;
+ cnt1 = 0;
+ }
+ break;
+
+ case 61:
+ /* Processing at 1st left half line detection */
+ led_out( 0x1 );
+ handle( 0 );
+ motor2( 0 ,0 );
+ pattern = 62;
+ cnt1 = 0;
+ break;
+
+ case 62:
+ /* Read but ignore 2nd time */
+ if( cnt1 > 100 ) {
+ pattern = 63;
+ cnt1 = 0;
+ break;
+ }
+ if( MarkCheck_Crossline( 90 ) ) { /* Cross line check */
+ led_m_set( MARK_C );
+ pattern = 21;
+ break;
+ }
+ break;
+
+ case 63:
+ /* Trace, lane change after left half line detection */
+ if( (digital_sensor()&0x1f) == 0x00 ) {
+ handle( -22 );
+ motor2( diff(25) ,35 );
+ pattern = 64;
+ cnt1 = 0;
+ break;
+ }
+ switch( (digital_sensor()&0x0f) ) {
+ case 0x00:
+ /* Center -> straight */
+ handle( 0 );
+ motor2( 35 ,35 );
+ break;
+ case 0x02:
+ /* Left of center -> turn to right */
+ handle( 4 );
+ motor2( 35 ,diff(35) );
+ break;
+ case 0x04:
+ /* Right of center -> turn to left */
+ handle( -4 );
+ motor2( diff(35) ,35 );
+ break;
+ default:
+ break;
+ }
+ break;
+
+ case 64:
+ if( cnt1 > 500 ){
+ pattern = 65;
+ cnt1 = 0;
+ }
+ break;
+
+ case 65:
+ /* Left lane change end check */
+ if( (digital_sensor()&0x02) == 0x02 ) {
+ led_out( 0x0 );
+ led_m_set( RUN );
+ pattern = 11;
+ cnt1 = 0;
+ }
+ break;
+
case 91:
- /* Stop */
handle( 0 );
motor( 0, 0 );
break;
@@ -541,6 +836,7 @@
/* top or bottom (Change) */
if ( vfield_count2 == 0 ) vfield_count2 = 1;
else if ( vfield_count2 == 1 ) vfield_count2 = 0;
+ led_m_user( vfield_count2 );
}
//******************************************************************//
@@ -587,17 +883,15 @@
void intTimer( void )
{
static int counter = 0;
-
+
cnt0++;
cnt1++;
/* field check */
- if( vfield_count2 != vfield_count2_buff ) {
+ if( vfield_count2 == vfield_count2_buff ) {
vfield_count2_buff = vfield_count2;
- counter = 0;
}
- /* Top field / bottom field */
- led_m_user( vfield_count2 );
+ /* Top field / bottom field */
switch( counter++ ) {
case 0:
Image_Extraction( write_buff_addr, ImageData_A, vfield_count2 );
@@ -622,10 +916,25 @@
//MarkCheck_Triangle
MarkDetect_process_T();
break;
+ case 7:
+ //MarkCheck_Crossline
+ MarkDetect_process_C();
+ break;
+ case 8:
+ //MarkCheck_Rightline
+ MarkDetect_process_R();
+ break;
+ case 9:
+ //MarkCheck_Leftline
+ MarkDetect_process_L();
+ break;
+ case 15:
+ counter = 0;
+ break;
default:
break;
+
}
-
/* LED processing */
led_m_process();
}
@@ -727,7 +1036,36 @@
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 )
@@ -1054,6 +1392,75 @@
}
}
+// MarkDetect_process_C
+//------------------------------------------------------------------//
+void MarkDetect_process_C( void )
+{
+ int x, y;
+
+ retJudgeIM_Max[1] = 0;
+ for( y = 0; y <= 13; y++ ) {
+ for( x = 0; x <= 6; x++ ) {
+ Image_part_Extraction( ImageBinary, 20, x, y, NowImageBinary, 14, 2 );
+ retDevi = Standard_Deviation( NowImageBinary, NowDevi, 28 );
+ retCovari = Covariance( TempDevi_Crossline, NowDevi, 28 );
+ retJudgeIM = 0;
+ retJudgeIM = Judgement_ImageMatching( retCovari, retDevi_Crossline, retDevi );
+ if( 100 >= retJudgeIM && retJudgeIM > retJudgeIM_Max[1] ) {
+ Xc = x;
+ Yc = y;
+ retJudgeIM_Max[1] = retJudgeIM;
+ }
+ }
+ }
+}
+
+// MarkDetect_process_R
+//------------------------------------------------------------------//
+void MarkDetect_process_R( void )
+{
+ int x, y;
+
+ retJudgeIM_Max[2] = 0;
+ for( y = 0; y <= 13; y++ ) {
+ for( x = 0; x <= 6; x++ ) {
+ Image_part_Extraction( ImageBinary, 20, x, y, NowImageBinary, 14, 2 );
+ retDevi = Standard_Deviation( NowImageBinary, NowDevi, 28 );
+ retCovari = Covariance( TempDevi_Rightline, NowDevi, 28 );
+ retJudgeIM = 0;
+ retJudgeIM = Judgement_ImageMatching( retCovari, retDevi_Rightline, retDevi );
+ if( 100 >= retJudgeIM && retJudgeIM > retJudgeIM_Max[2] ) {
+ Xr = x;
+ Yr = y;
+ retJudgeIM_Max[2] = retJudgeIM;
+ }
+ }
+ }
+}
+
+// MarkDetect_process_L
+//------------------------------------------------------------------//
+void MarkDetect_process_L( void )
+{
+ int x, y;
+
+ retJudgeIM_Max[3] = 0;
+ for( y = 0; y <= 13; y++ ) {
+ for( x = 0; x <= 6; x++ ) {
+ Image_part_Extraction( ImageBinary, 20, x, y, NowImageBinary, 14, 2 );
+ retDevi = Standard_Deviation( NowImageBinary, NowDevi, 28 );
+ retCovari = Covariance( TempDevi_Leftline, NowDevi, 28 );
+ retJudgeIM = 0;
+ retJudgeIM = Judgement_ImageMatching( retCovari, retDevi_Leftline, retDevi );
+ if( 100 >= retJudgeIM && retJudgeIM > retJudgeIM_Max[3] ) {
+ Xl = x;
+ Yl = y;
+ retJudgeIM_Max[3] = retJudgeIM;
+ }
+ }
+ }
+}
+
// MarkCheck Triangle detection
// Return values: 0: no triangle mark, 1: Triangle mark
//------------------------------------------------------------------//
@@ -1069,6 +1476,51 @@
return ret;
}
+// Cross line detection processing
+// Return values: 0: no cross line, 1: cross line
+//------------------------------------------------------------------//
+int MarkCheck_Crossline( int percentage )
+{
+ int ret;
+
+ ret = 0;
+ if( retJudgeIM_Max[1] >= percentage ) {
+ ret = 1;
+ }
+
+ return ret;
+}
+
+// Right half line detection processing
+// Return values: 0: not detected, 1: detected
+//------------------------------------------------------------------//
+int MarkCheck_Rightline( int percentage )
+{
+ int ret;
+
+ ret = 0;
+ if( retJudgeIM_Max[2] >= percentage ) {
+ ret = 1;
+ }
+
+ return ret;
+}
+
+// Left half line detection processing
+// Return values: 0: not detected, 1: detected
+//------------------------------------------------------------------//
+int MarkCheck_Leftline( int percentage )
+{
+ int ret;
+
+ ret = 0;
+ if( retJudgeIM_Max[3] >= percentage ) {
+ ret = 1;
+ }
+
+ return ret;
+}
+
//******************************************************************//
// Debug functions
//*******************************************************************/
@@ -1105,8 +1557,11 @@
pc.printf( "\n\r" );
pc.printf( "sensor_inp = 0x%02x\n\r", digital_sensor() );
pc.printf( "T = %3d%% %01d X=%2d Y=%2d\n\r", retJudgeIM_Max[0], MarkCheck_Triangle( 90 ), Xt, Yt );
+ pc.printf( "C = %3d%% %01d X=%2d Y=%2d\n\r", retJudgeIM_Max[1], MarkCheck_Crossline( 90 ), Xc, Yc );
+ pc.printf( "R = %3d%% %01d X=%2d Y=%2d\n\r", retJudgeIM_Max[2], MarkCheck_Rightline( 90 ), Xr, Yr );
+ pc.printf( "L = %3d%% %01d X=%2d Y=%2d\n\r", retJudgeIM_Max[3], MarkCheck_Leftline( 90 ), Xl, Yl );
pc.printf( "\n\r" );
- Height += 4;
+ Height += 7;
pc.printf( "\033[%dA" , Height );
}