Revision 2:af4db183ee76, committed 2017-03-17

Comitter:

TetsuyaKonno

Date:

Fri Mar 17 03:53:01 2017 +0000

Parent:

1:4a94c87a7d04

Commit message:

Added program for mark detection (Cross line, Left half line and Right half line).

Changed in this revision

--- 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 );
 }

--- a/mbed.bld	Tue Jan 10 04:59:48 2017 +0000
+++ b/mbed.bld	Fri Mar 17 03:53:01 2017 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/abea610beb85
\ No newline at end of file
+http://mbed.org/users/mbed_official/code/mbed/builds/093f2bd7b9eb
\ No newline at end of file