Micon Car Rally
This program supports the Image processing micon car production kit (M-S348).
Dependencies: GR-PEACH_video mbed
Revision 0:00b6f7454ada, committed 2018-10-30
- Comitter:
- TetsuyaKonno
- Date:
- Tue Oct 30 09:06:30 2018 +0000
- Commit message:
- New program file
Changed in this revision
diff -r 000000000000 -r 00b6f7454ada GR-PEACH_video.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/GR-PEACH_video.lib Tue Oct 30 09:06:30 2018 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/teams/Renesas/code/GR-PEACH_video/#aeefe5171463
diff -r 000000000000 -r 00b6f7454ada image_process.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/image_process.cpp Tue Oct 30 09:06:30 2018 +0000
@@ -0,0 +1,363 @@
+//------------------------------------------------------------------//
+//Supported MCU: RZ/A1H
+//File Contents: Image Processing API ( Source file )
+//Version number: Ver.1.00
+//Date: 2018.10.30
+//Copyright: Renesas Electronics Corporation
+// Hitachi Document Solutions Co., Ltd.
+//------------------------------------------------------------------//
+
+//------------------------------------------------------------------//
+//Include
+//------------------------------------------------------------------//
+#include <math.h>
+#include "mbed.h"
+#include "image_process.h"
+
+/*******************************/
+/* Function Map */
+/********************************
+* ImageCopy
+* Extraction_Brightness
+* ImageReduction
+* Binarization
+* Image_part_Extraction
+* Standard_Deviation
+* Covariance
+* Judgement_ImageMatching
+* PatternMatching_process
+********************************/
+
+//******************************************************************//
+// Image process functions
+//*******************************************************************/
+//------------------------------------------------------------------//
+// ImageCopy function
+//------------------------------------------------------------------//
+void ImageCopy( unsigned char *BuffAddrIn, int HW, int VW, unsigned char *BuffAddrOut, int Frame )
+{
+ static int counter = 0;
+ static int X, Y;
+ int HW_T;//HW Twice
+
+ HW_T = HW + HW;
+
+ switch( counter++ ) {
+ case 0:
+ // Top or Bottom field
+ for( Y = Frame; Y < ( VW / 2 ); Y+=2 ){
+ for( X = 0; X < HW_T; X++ ){
+ BuffAddrOut[ ( Y * HW_T ) + X ] = BuffAddrIn[ ( Y * HW_T ) + X ];
+ }
+ }
+ break;
+ case 1:
+ // Top or Bottom field
+ for( ; Y < VW; Y+=2 ){
+ for( X = 0; X < HW_T; X++ ){
+ BuffAddrOut[ ( Y * HW_T ) + X ] = BuffAddrIn[ ( Y * HW_T ) + X ];
+ }
+ }
+ //Frame Change
+ if( Frame == 0 ) Frame = 1;
+ else Frame = 0;
+ for( Y = Frame; Y < VW; Y+=2 ){
+ for( X = 0; X < HW_T; X+=2 ){
+ BuffAddrOut[ ( Y * HW_T ) + ( X + 0 ) ] = 0;
+ BuffAddrOut[ ( Y * HW_T ) + ( X + 1 ) ] = 128;
+ }
+ }
+ counter = 0;
+ break;
+ default:
+ break;
+ }
+}
+
+//------------------------------------------------------------------//
+// Extraction Brightness function
+//------------------------------------------------------------------//
+void Extraction_Brightness( unsigned char *BuffAddrIn, int HW, int VW, unsigned char *BuffAddrOut, int Frame )
+{
+ static int conter = 0;
+
+ int X, Y;
+ int Px;
+ int HW_T;//HW Twice
+
+ HW_T = HW + HW;
+
+ switch( conter++ ) {
+ case 0:
+ //Pixel Interpolation ( Top or Bottom )
+ for( Y = Frame; Y < ( VW / 2 ); Y+=2 ){
+ for( X = 0, Px = 0; X < HW_T; X+=2, Px++ ){
+ BuffAddrOut[ ( Y * HW ) + Px ] = BuffAddrIn[ ( Y * HW_T ) + X ];
+ }
+ }
+ //Frame Change
+ if( Frame == 0 ) Frame = 1;
+ else Frame = 0;
+ //Bilinear Interpolation Method
+ for( Y = Frame; Y < ( VW / 2 ); Y+=2 ){
+ for( X = 0, Px = 0; X < HW_T; X+=2, Px++ ){
+ if( Y <= 0 ) {
+ BuffAddrOut[ ( Y * HW ) + Px ] = BuffAddrOut[ ( (Y+1) * HW ) + Px ];
+ } else if( ( ( VW / 2 ) - 1 ) > Y && Y > 0 ) {
+ BuffAddrOut[ ( Y * HW ) + Px ] = ( BuffAddrOut[ ( (Y-1) * HW ) + Px ] * (double)0.5 ) + ( BuffAddrOut[ ( (Y+1) * HW ) + Px ] * (double)0.5 );
+ } else if( Y >= ( ( VW / 2 ) - 1 ) ) {
+ BuffAddrOut[ ( Y * HW ) + Px ] = BuffAddrOut[ ( (Y-1) * HW ) + Px ];
+ }
+ }
+ }
+ break;
+ case 1:
+ //Pixel Interpolation ( Top or Bottom )
+ for( Y = ( VW / 2 ) + Frame; Y < VW; Y+=2 ){
+ for( X = 0, Px = 0; X < HW_T; X+=2, Px++ ){
+ BuffAddrOut[ ( Y * HW ) + Px ] = BuffAddrIn[ ( Y * HW_T ) + X ];
+ }
+ }
+ //Frame Change
+ if( Frame == 0 ) Frame = 1;
+ else Frame = 0;
+ //Bilinear Interpolation Method
+ for( Y = ( VW / 2 ) + Frame; Y < VW; Y+=2 ){
+ for( X = 0, Px = 0; X < HW_T; X+=2, Px++ ){
+ if( Y <= 0 ) {
+ BuffAddrOut[ ( Y * HW ) + Px ] = BuffAddrOut[ ( (Y+1) * HW ) + Px ];
+ } else if( ( VW - 1 ) > Y && Y > 0 ) {
+ BuffAddrOut[ ( Y * HW ) + Px ] = ( BuffAddrOut[ ( (Y-1) * HW ) + Px ] * (double)0.5 ) + ( BuffAddrOut[ ( (Y+1) * HW ) + Px ] * (double)0.5 );
+ } else if( Y >= ( VW - 1 ) ) {
+ BuffAddrOut[ ( Y * HW ) + Px ] = BuffAddrOut[ ( (Y-1) * HW ) + Px ];
+ }
+ }
+ }
+ conter = 0;
+ break;
+ default:
+ break;
+ }
+}
+
+//------------------------------------------------------------------//
+// Image Reduction function
+// Parcent : 0.0 - 1.0
+//------------------------------------------------------------------//
+void ImageReduction( unsigned char *BuffAddrIn, int HW, int VW, unsigned char *BuffAddrOut, double Percent )
+{
+ static int conter = 0;
+ static int Y;
+
+ int X;
+ int HW_N;
+ long Nx, Ny;
+ long NxBuff, NyBuff;
+ unsigned int BuffAddrData;
+ double long Sx, Sy;
+
+ NxBuff = -1;
+ Sx = Sy = Percent;
+ HW_N = HW * Percent;
+
+ //Under 100%
+ if( Percent <= 1 ) {
+ switch( conter++ ) {
+ case 0:
+ for( Y = 0; Y < ( VW / 2 ); Y++ ){
+ //Affine Transformation Y-axis
+ Ny = ( Sy * Y );
+ for( X = 0; X < HW; X++ ){
+ //Affine Transformation X-axis
+ Nx = ( Sx * X );
+ if( NxBuff == Nx ) {
+ BuffAddrData = BuffAddrOut[ ( Ny * HW_N ) + Nx ];
+ BuffAddrData += BuffAddrIn[ ( Y * HW ) + X ];
+ BuffAddrData /= 2;
+ BuffAddrOut[ ( Ny * HW_N ) + Nx ] = BuffAddrData;
+ } else {
+ NxBuff = Nx;
+ BuffAddrOut[ ( Ny * HW_N ) + Nx ] = BuffAddrIn[ ( Y * HW ) + X ];
+ }
+ }
+ if( NyBuff == Ny ) {
+ for( X = 0; X < HW_N; X++ ){
+ BuffAddrData = BuffAddrOut[ ( Ny * HW_N ) + X ];//Now line
+ BuffAddrData += BuffAddrOut[ ( (Ny - 1) * HW_N ) + X ];//Before now line
+ BuffAddrData /= 2;
+ BuffAddrOut[ ( Ny * HW_N ) + X ] = BuffAddrData;
+ }
+ } else {
+ NyBuff = Ny;
+ }
+ }
+ break;
+ case 1:
+ for( ; Y < VW; Y++ ){
+ //Affine Transformation Y-axis
+ Ny = ( Sy * Y );
+ for( X = 0; X < HW; X++ ){
+ //Affine Transformation X-axis
+ Nx = ( Sx * X );
+ if( NxBuff == Nx ) {
+ BuffAddrData = BuffAddrOut[ ( Ny * HW_N ) + Nx ];
+ BuffAddrData += BuffAddrIn[ ( Y * HW ) + X ];
+ BuffAddrData /= 2;
+ BuffAddrOut[ ( Ny * HW_N ) + Nx ] = BuffAddrData;
+ } else {
+ NxBuff = Nx;
+ BuffAddrOut[ ( Ny * HW_N ) + Nx ] = BuffAddrIn[ ( Y * HW ) + X ];
+ }
+ }
+ if( NyBuff == Ny ) {
+ for( X = 0; X < HW_N; X++ ){
+ BuffAddrData = BuffAddrOut[ ( Ny * HW_N ) + X ];//Now line
+ BuffAddrData += BuffAddrOut[ ( (Ny - 1) * HW_N ) + X ];//Before now line
+ BuffAddrData /= 2;
+ BuffAddrOut[ ( Ny * HW_N ) + X ] = BuffAddrData;
+ }
+ } else {
+ NyBuff = Ny;
+ }
+ }
+ conter = 0;
+ break;
+ default:
+ break;
+ }
+ }
+}
+
+//------------------------------------------------------------------//
+// Binarization process Function
+//------------------------------------------------------------------//
+void Binarization( unsigned char *BuffAddrIn, int HW, int VW, unsigned char *BuffAddrOut, int threshold )
+{
+ int i;
+ int items;
+
+ items = HW * VW;
+
+ for( i = 0; i < items; i++ ) {
+ if( BuffAddrIn[i] >= threshold ) BuffAddrOut[i] = 1;
+ else BuffAddrOut[i] = 0;
+ }
+}
+
+//******************************************************************//
+// Mark detect functions
+//*******************************************************************/
+//------------------------------------------------------------------//
+// Extract_Image
+//------------------------------------------------------------------//
+void Image_part_Extraction( unsigned char *BuffAddrIn, int HW, int VW,
+ int CutPointX, int CutPointY, unsigned char *BuffAddrOut, int Xsize, int Ysize )
+{
+ int X, Y;
+ for( Y = 0; Y < Ysize; Y++ ) {
+ for( X = 0; X < Xsize; X++ ) {
+ BuffAddrOut[ X + ( Y * Xsize ) ] = BuffAddrIn[ ( ( Y + CutPointY ) * HW ) + ( X + CutPointX ) ];
+ }
+ }
+}
+
+//------------------------------------------------------------------//
+// Standard deviation
+//------------------------------------------------------------------//
+double Standard_Deviation( unsigned char *data, double *Devi, int Xsize, int Ysize )
+{
+ int i;
+ int items;
+ double iRet_A, iRet_C, iRet_D;
+
+ items = Xsize * Ysize;
+
+ /* 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 Xsize, int Ysize )
+{
+ int i;
+ int items;
+ double iRet, iRet_buff;
+
+ items = Xsize * Ysize;
+
+ 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;
+}
+
+//------------------------------------------------------------------//
+// Pattern Matching process
+//------------------------------------------------------------------//
+void PatternMatching_process( unsigned char *BuffAddrIn, int HW, int VW,
+ ImagePartPattern *Temp, int Xs, int Xe, int Ys, int Ye )
+{
+ ImagePartPattern NowImage;
+ volatile int x, y;
+ volatile int retJudge;
+ volatile double retCovari;
+
+ Temp->p = 0;
+ for( y = Ys; y <= Ye; y++ ) {
+ for( x = Xs; x <= Xe; x++ ) {
+ Image_part_Extraction( BuffAddrIn, HW, VW, x, y, NowImage.binary, Temp->w, Temp->h );
+ NowImage.sdevi = Standard_Deviation( NowImage.binary, NowImage.devi, Temp->w, Temp->h);
+ retCovari = Covariance( Temp->devi, NowImage.devi, Temp->w, Temp->h );
+ retJudge = 0;
+ retJudge = Judgement_ImageMatching( retCovari, Temp->sdevi, NowImage.sdevi );
+ if( 100 >= retJudge && retJudge > Temp->p ) {
+ Temp->x = x;
+ Temp->y = y;
+ Temp->p = retJudge;
+ }
+ }
+ }
+}
+
+//------------------------------------------------------------------//
+// End of file
+//------------------------------------------------------------------//
\ No newline at end of file
diff -r 000000000000 -r 00b6f7454ada image_process.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/image_process.h Tue Oct 30 09:06:30 2018 +0000
@@ -0,0 +1,42 @@
+//------------------------------------------------------------------//
+//Supported MCU: RZ/A1H
+//File Contents: Image Processing API ( Header file )
+//Version number: Ver.1.00
+//Date: 2018.10.30
+//Copyright: Renesas Electronics Corporation
+// Hitachi Document Solutions Co., Ltd.
+//------------------------------------------------------------------//
+//Struct
+//------------------------------------------------------------------//
+typedef struct {
+ volatile int p; //percent
+ volatile int x; //Point X
+ volatile int y; //Point Y
+ volatile double sdevi; //Standard_Deviation
+ double devi[100]; //Deviation
+ unsigned char binary[100]; //Binary
+ volatile int w; //Binary Width pixel
+ volatile int h; //Binary Height pixel
+} ImagePartPattern;
+
+//------------------------------------------------------------------//
+//Prototype( Image process )
+//------------------------------------------------------------------//
+void ImageCopy( unsigned char *BuffAddrIn, int HW, int VW, unsigned char *BuffAddrOut, int Frame );
+void Extraction_Brightness( unsigned char *BuffAddrIn, int HW, int VW, unsigned char *BuffAddrOut, int Frame );
+void ImageReduction( unsigned char *BuffAddrIn, int HW, int VW, unsigned char *BuffAddrOut, double Percent );
+void Binarization( unsigned char *BuffAddrIn, int HW, int VW, unsigned char *BuffAddrOut, int threshold );
+
+//------------------------------------------------------------------//
+//Prototype( Mark detection process )
+//------------------------------------------------------------------//
+void Image_part_Extraction( unsigned char *BuffAddrIn, int HW, int VW,
+ int CutPointX, int CutPointY, unsigned char *BuffAddrOut, int Xsize, int Ysize );
+double Standard_Deviation( unsigned char *data, double *Devi, int Xsize, int Ysize );
+double Covariance( double *Devi_A, double *Devi_B, int Xsize, int Ysize );
+int Judgement_ImageMatching( double Covari, double SDevi_A, double SDevi_B );
+void PatternMatching_process( unsigned char *BuffAddrIn, int HW, int VW,
+ ImagePartPattern *Temp, int Xs, int Xe, int Ys, int Ye );
+//------------------------------------------------------------------//
+// End of file
+//------------------------------------------------------------------//
\ No newline at end of file
diff -r 000000000000 -r 00b6f7454ada main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Oct 30 09:06:30 2018 +0000
@@ -0,0 +1,1388 @@
+//------------------------------------------------------------------//
+//Supported MCU: RZ/A1H
+//File Contents: kit18 GR-peach ( Trace program )
+//Version number: Ver.1.00
+//Date: 2018.10.30
+//Copyright: Renesas Electronics Corporation
+// Hitachi Document Solutions Co., Ltd.
+//------------------------------------------------------------------//
+
+//This program supports the following kit:
+//* M-S348 Image processing micon car production kit
+
+//------------------------------------------------------------------//
+//Include
+//------------------------------------------------------------------//
+#include "mbed.h"
+#include "iodefine.h"
+#include "DisplayBace.h"
+#include "image_process.h"
+
+//------------------------------------------------------------------//
+//Define
+//------------------------------------------------------------------//
+//Motor PWM cycle
+#define MOTOR_PWM_CYCLE 33332 /* Motor PWM period */
+ /* 1ms P0φ/1 = 0.03us */
+//Servo PWM cycle
+#define SERVO_PWM_CYCLE 33332 /* SERVO PWM period */
+ /* 16ms P0φ/16 = 0.48us */
+#define SERVO_CENTER 3124 /* 1.5ms / 0.48us - 1 = 3124*/
+#define HANDLE_STEP 18 /* 1 degree value */
+
+#define THRESHOLD 128 /* Binarization function only */
+
+//LED Color on GR-PEACH
+#define LED_OFF 0x00
+#define LED_RED 0x01
+#define LED_GREEN 0x02
+#define LED_YELLOW 0x03
+#define LED_BLUE 0x04
+#define LED_PURPLE 0x05
+#define LED_SKYBLUE 0x06
+#define LED_WHITE 0x07
+
+//led_m_set function only
+#define RUN 0x00
+#define STOP 0x01
+#define ERROR 0x02
+#define DEBUG 0x03
+#define CRANK 0x04
+#define LCHANGE 0x05
+
+//ImageData_Serial_out3 function only
+#define COLOR 0x01
+#define GREY_SCALE 0x02
+#define BINARY 0x03
+
+//------------------------------------------------------------------//
+//Define(NTSC-Video)
+//------------------------------------------------------------------//
+#define VIDEO_INPUT_CH (DisplayBase::VIDEO_INPUT_CHANNEL_0)
+#define VIDEO_INT_TYPE (DisplayBase::INT_TYPE_S0_VFIELD)
+#define DATA_SIZE_PER_PIC (2u)
+
+/*! Frame buffer stride: Frame buffer stride should be set to a multiple of 32 or 128
+ in accordance with the frame buffer burst transfer mode. */
+#define PIXEL_HW (160u) /* QVGA */
+#define PIXEL_VW (120u) /* QVGA */
+#define VIDEO_BUFFER_STRIDE (((PIXEL_HW * DATA_SIZE_PER_PIC) + 31u) & ~31u)
+#define VIDEO_BUFFER_HEIGHT (PIXEL_VW)
+
+//------------------------------------------------------------------//
+//Constructor
+//------------------------------------------------------------------//
+/* Create DisplayBase object */
+DisplayBase Display;
+
+Ticker interrput;
+Serial pc(USBTX, USBRX);
+
+DigitalOut LED_R(P6_13); /* LED1 on the GR-PEACH board */
+DigitalOut LED_G(P6_14); /* LED2 on the GR-PEACH board */
+DigitalOut LED_B(P6_15); /* LED3 on the GR-PEACH board */
+DigitalOut USER_LED(P6_12); /* USER_LED on the GR-PEACH board */
+DigitalIn user_botton(P6_0); /* SW1 on the GR-PEACH board */
+
+BusIn dipsw( P7_15, P8_1, P2_9, P2_10 ); /* SW1 on Shield board */
+
+DigitalOut Left_motor_signal(P4_6); /* Used by motor function */
+DigitalOut Right_motor_signal(P4_7); /* Used by motor function */
+DigitalIn push_sw(P2_13); /* SW1 on the Motor Drive board */
+DigitalOut LED_3(P2_14); /* LED3 on the Motor Drive board */
+DigitalOut LED_2(P2_15); /* LED2 on the Motor Drive board */
+
+//------------------------------------------------------------------//
+//Prototype(NTSC-video)
+//------------------------------------------------------------------//
+void init_Camera( void );
+void ChangeFrameBuffer( void );
+static void IntCallbackFunc_Vfield(DisplayBase::int_type_t int_type);
+static void WaitVfield(const int32_t wait_count);
+static void IntCallbackFunc_Vsync(DisplayBase::int_type_t int_type);
+static void WaitVsync(const int32_t wait_count);
+
+//------------------------------------------------------------------//
+//Prototype
+//------------------------------------------------------------------//
+//Peripheral functions
+void init_MTU2_PWM_Motor( void ); /* Initialize PWM functions */
+void init_MTU2_PWM_Servo( void ); /* Initialize PWM functions */
+
+//Interrupt function
+void intTimer( void ); /* 1ms period */
+
+//GR-peach board
+void led_rgb(int led);
+void led_m_user( int led );
+unsigned char user_button_get( void );
+void led_m_set( int set );
+void led_m_process( void ); /* Only function for interrupt */
+
+//Motor drive board
+void led_out(int led);
+unsigned char pushsw_get( void );
+void motor( int accele_l, int accele_r );
+void motor2( int accele_l, int accele_r );
+void handle( int angle );
+int diff( int pwm );
+
+//Shield board
+unsigned char dipsw_get( void );
+
+//------------------------------------------------------------------//
+//Prototype( Digital sensor process )
+//------------------------------------------------------------------//
+int init_sensor( unsigned char *BuffAddrIn, int HW, int VW, int Cx, int *SENPx, int Y );
+unsigned char sensor_process( unsigned char *BuffAddrIn, int HW, int VW, int *SENPx, int Y ); /* Only function for interrupt */
+unsigned char sensor_inp( void );
+unsigned char center_inp( void );
+
+//------------------------------------------------------------------//
+//Prototype( Mark detect functions )
+//------------------------------------------------------------------//
+int RightCrankCheck( int percentage );
+int RightLaneChangeCheck( int percentage );
+
+//------------------------------------------------------------------//
+//Prototype( Debug functions )
+//------------------------------------------------------------------//
+void ImageData_Serial_Out1( unsigned char *ImageData, int HW, int VW );
+void ImageData_Serial_Out2( unsigned char *ImageData, int HW, int VW );
+void ImageData_Serial_Out3( unsigned char *ImageData, int HW, int VW, int color_pattern );
+
+//------------------------------------------------------------------//
+//Global variable (NTSC-video)
+//------------------------------------------------------------------//
+static uint8_t FrameBuffer_Video_A[VIDEO_BUFFER_STRIDE * VIDEO_BUFFER_HEIGHT]__attribute((section("NC_BSS"),aligned(16))); //16 bytes aligned!;
+static uint8_t FrameBuffer_Video_B[VIDEO_BUFFER_STRIDE * VIDEO_BUFFER_HEIGHT]__attribute((section("NC_BSS"),aligned(16))); //16 bytes aligned!;
+uint8_t * write_buff_addr = FrameBuffer_Video_A;
+uint8_t * save_buff_addr = FrameBuffer_Video_B;
+static volatile int32_t vsync_count;
+static volatile int32_t vfield_count;
+static volatile int32_t vfield_count2 = 1;
+static volatile int32_t vfield_count2_buff;
+
+//------------------------------------------------------------------//
+//Global variable for Image process
+//------------------------------------------------------------------//
+unsigned char ImageData_A[ ( ( PIXEL_HW * 2) * PIXEL_VW ) ];
+unsigned char ImageData_B[ ( PIXEL_HW * PIXEL_VW ) ];
+unsigned char ImageComp_B[ ( PIXEL_HW * PIXEL_VW ) ];
+unsigned char ImageBinary[ ( PIXEL_HW * PIXEL_VW ) ];
+
+double Rate = 0.125; /* Reduction ratio */
+
+//------------------------------------------------------------------//
+//Global variable for Digital sensor function
+//------------------------------------------------------------------//
+int SenError;
+int Sen1Px[5];
+unsigned char SenVal1;
+
+//------------------------------------------------------------------//
+//Global variable for Mark detection function
+//------------------------------------------------------------------//
+ImagePartPattern RightCrank = {0,0,0,0,{0}, //percent, Point X, Point Y, Standard_Deviation, Deviation
+ {0,0,0,0,0,0,0, //Binary image
+ 0,0,1,1,1,1,1, //Binary image
+ 0,0,1,1,1,0,0, //Binary image
+ 0,0,1,1,1,0,0},//Binary image
+ 7, 4}; //Binary Width pixel, Binary Height pixel
+
+ImagePartPattern RightLaneChange = {0,0,0,0,{0},//percent, Point X, Point Y, Standard_Deviation, Deviation
+ {0,0,0,1,1,0,0,0,0,0,0,0,0,0, //Binary image
+ 0,0,0,1,1,0,0,0,0,0,0,0,0,0, //Binary image
+ 0,0,0,1,1,0,0,0,0,0,0,0,0,0},//Binary image
+ 14, 3}; //Binary Width pixel, Binary Height pixel
+
+//------------------------------------------------------------------//
+//Global variable for Trace program
+//------------------------------------------------------------------//
+volatile unsigned long cnt0; /* Used by timer function */
+volatile unsigned long cnt1; /* Used within main */
+volatile int pattern; /* Pattern numbers */
+
+volatile int led_pattern; /* led_m_process function only */
+volatile int initFlag; /* Initialize flag */
+volatile int threshold_buff; /* Binarization function only */
+volatile int handle_buff; /* diff function only */
+
+const int revolution_difference[] = { /* diff function only */
+ 100, 98, 97, 95, 93,
+ 92, 90, 88, 87, 85,
+ 84, 82, 81, 79, 78,
+ 76, 75, 73, 72, 71,
+ 69, 68, 66, 65, 64,
+ 62, 61, 59, 58, 57,
+ 55, 54, 52, 51, 50,
+ 48, 47, 45, 44, 42,
+ 41, 39, 38, 36, 35,
+ 33 };
+
+//******************************************************************//
+// Main function
+//*******************************************************************/
+int main( void )
+{
+ volatile int Number; /* Serial Debug Mode only */
+
+ initFlag = 1; /* Initialization start */
+
+ /* Camera start */
+ init_Camera();
+ /* wait to stabilize NTSC signal (about 170ms) */
+ wait(0.2);
+
+ /* Initialize MCU functions */
+ init_MTU2_PWM_Motor();
+ init_MTU2_PWM_Servo();
+ interrput.attach(&intTimer, 0.001);
+ pc.baud(230400);
+
+ /* Initialize Micon Car state */
+ handle( 0 );
+ motor( 0, 0 );
+ led_out( 0x0 );
+ led_m_set( STOP );
+
+ threshold_buff = THRESHOLD;
+
+ wait(0.5);
+
+ /* Initialize Digital sensor */
+ SenError = init_sensor( ImageBinary, (PIXEL_HW * Rate), (PIXEL_VW * Rate), (PIXEL_HW * Rate) / 2, Sen1Px, 12 );
+ if( SenError !=0 ) {
+ led_m_set( ERROR );
+ cnt1 = 0;
+ while( cnt1 < 3000 ){
+ if( cnt1 % 200 < 100 ) {
+ led_out( 0x3 );
+ } else {
+ led_out( 0x0 );
+ }
+ }
+ }
+
+ /* Initialize Pattern Matching */
+ RightCrank.sdevi = Standard_Deviation( RightCrank.binary, RightCrank.devi, RightCrank.w, RightCrank.h );
+ RightLaneChange.sdevi = Standard_Deviation( RightLaneChange.binary, RightLaneChange.devi, RightLaneChange.w, RightLaneChange.h );
+
+ initFlag = 0; /* Initialization end */
+
+ /* Debug Program */
+ if( user_button_get() ) {
+ led_m_set( DEBUG );
+ wait(0.1);//ON Time
+ while( user_button_get() );
+ wait(0.5);//OFF Time
+ while( 1 ){
+ pc.printf( "Serial Debug Mode Ver1.0 (2018.10.30)\n\r" );
+ pc.printf( "\n\r" );
+ pc.printf( "0:TeraTram Real-time display 20* 15pixel (Binary)\n\r" );
+ pc.printf( "1:Excel(csv) 160*120pixel\n\r" );
+ pc.printf( "2:Excel(csv) 20* 15pixel\n\r" );
+ pc.printf( "3:Excel(csv) 160*120pixel -> csv_jpg_convert.bat (color)\n\r" );
+ pc.printf( "4:Excel(csv) 160*120pixel -> csv_jpg_convert.bat (grey scale)\n\r" );
+ pc.printf( "5:Excel(csv) 20* 15pixel -> csv_jpg_convert.bat (binary)\n\r" );
+ pc.printf( "\n\r" );
+ pc.printf( "Please Number\n\r" );
+ pc.printf( "No = " );
+ pc.scanf( "%d", &Number );
+ pc.printf( "\n\r" );
+ pc.printf( "Please push the SW ( on the Motor drive board )\n\r" );
+ pc.printf( "\n\r" );
+ while( !pushsw_get() );
+
+ switch( Number ) {
+ case 0:
+ /* for TeraTerm(Real-time display) */
+ while( 1 ) {
+ ImageData_Serial_Out2( ImageBinary, (PIXEL_HW * Rate), (PIXEL_VW * Rate) );
+ }
+ break;
+ case 1:
+ /* for the Excel(csv) 160*120pixel */
+ ImageData_Serial_Out1( ImageData_B, PIXEL_HW, PIXEL_VW );
+ break;
+ case 2:
+ /* for the Excel(csv) 20* 15pixel */
+ ImageData_Serial_Out1( ImageComp_B, (PIXEL_HW * Rate), (PIXEL_VW * Rate) );
+ break;
+ case 3:
+ /* for the Excel(csv) -> csv_jpg_convert.bat */
+ ChangeFrameBuffer();
+ ImageData_Serial_Out3( save_buff_addr, PIXEL_HW, PIXEL_VW, COLOR );
+ break;
+ case 4:
+ /* for the Excel(csv) -> csv_jpg_convert.bat */
+ ImageData_Serial_Out3( ImageData_B, PIXEL_HW, PIXEL_VW, GREY_SCALE );
+ break;
+ case 5:
+ /* for the Excel(csv) -> csv_jpg_convert.bat */
+ ImageData_Serial_Out3( ImageBinary, (PIXEL_HW * Rate), (PIXEL_VW * Rate), BINARY );
+ break;
+ default:
+ break;
+ }
+ led_m_set( STOP );
+ while(1);
+ }
+ }
+
+ /* Trace program */
+ led_m_set( RUN );
+
+ while( 1 ) {
+
+ switch( pattern ) {
+ /*****************************************************************
+ About patern
+ 0:wait for switch input
+ 1:check if start bar is open
+ 11:normal trace
+ 12:Left side
+ 13:right side
+ *****************************************************************/
+ case 0:
+ /* wait for switch input */
+ if( pushsw_get() ) {
+ led_out( 0x0 );
+ led_m_set( RUN );
+ pattern = 11;
+ cnt1 = 0;
+ break;
+ }
+ if( cnt1 < 100 ) {
+ led_out( 0x1 );
+ } else if( cnt1 < 200 ) {
+ led_out( 0x2 );
+ } else {
+ cnt1 = 0;
+ }
+ break;
+
+ case 11:
+ /* normal trace */
+ if( RightCrankCheck( 85 ) ) { /* Right Crank Check */
+ pattern = 31;
+ break;
+ }
+ if( RightLaneChangeCheck( 85 ) ) { /* Right Lane Change Check */
+ pattern = 51;
+ break;
+ }
+ switch( (sensor_inp()&0x0f) ) {
+ case 0x00:
+ handle( 0 );
+ motor( 100, 100 );
+ break;
+ case 0x02:
+ handle( 3 );
+ motor( 100, diff(100) );
+ break;
+ case 0x03:
+ handle( 12 );
+ motor( 100, diff(100) );
+ break;
+ case 0x01:
+ handle( 20 );
+ motor( 100, diff(100) );
+ pattern = 12;
+ break;
+ case 0x04:
+ handle( -3 );
+ motor( diff(100), 100 );
+ break;
+ case 0x0c:
+ handle( -12 );
+ motor( diff(100), 100 );
+ break;
+ case 0x08:
+ handle( -20 );
+ motor( diff(100), 100 );
+ pattern = 13;
+ break;
+ default:
+ break;
+ }
+ break;
+
+ case 12:
+ /* Left side */
+ if( (sensor_inp()&0x02) == 0x02 ) {
+ pattern = 11;
+ break;
+ }
+ switch( (sensor_inp()&0x0f) ) {
+ case 0x01:
+ handle( 20 );
+ motor( 100, diff(100) );
+ break;
+ case 0x00:
+ case 0x08:
+ case 0x0c:
+ handle( 22 );
+ motor( 100, diff(100) );
+ break;
+ default:
+ break;
+ }
+ break;
+
+ case 13:
+ /* right side */
+ if( (sensor_inp()&0x04) == 0x04 ) {
+ pattern = 11;
+ }
+ switch( (sensor_inp()&0x0f) ) {
+ case 0x08:
+ handle( -20 );
+ motor( diff(100), 100 );
+ break;
+ case 0x00:
+ case 0x01:
+ case 0x03:
+ handle( -22 );
+ motor( diff(100), 100 );
+ break;
+ default:
+ break;
+ }
+ break;
+
+ case 31:
+ /* Right crank processing at 1st process */
+ led_m_set( CRANK );
+ led_out( 0x1 );
+ handle( 0 );
+ motor2( 0 ,0 );
+ pattern = 32;
+ cnt1 = 0;
+ break;
+
+ case 32:
+ /* Right crank processing at 2nd process */
+ if( cnt1 > 300 ) {
+ pattern = 33;
+ cnt1 = 0;
+ }
+ break;
+
+ case 33:
+ /* Right crank processing at 3rd process */
+ if( ( center_inp() == 0 ) && ( (sensor_inp()&0x0f) == 0x00 ) ) {
+ handle( 41 );
+ motor2( 50 ,10 );
+ pattern = 34;
+ cnt1 = 0;
+ break;
+ }
+ switch( (sensor_inp()&0x0f) ) {
+ case 0x00:
+ /* Center -> straight */
+ handle( 0 );
+ motor2( 30 ,30 );
+ break;
+ case 0x02:
+ /* Left of center -> turn to right */
+ handle( 5 );
+ motor2( 30 ,diff(30) );
+ break;
+ case 0x04:
+ /* Right of center -> turn to left */
+ handle( -5 );
+ motor2( diff(30) ,30 );
+ break;
+ default:
+ break;
+ }
+ break;
+
+ case 34:
+ /* Right crank clearing processing - wait until stable */
+ if( cnt1 > 800 ) {
+ pattern = 35;
+ cnt1 = 0;
+ }
+ break;
+
+ case 35:
+ /* Right crank clearing processing - check end of turn */
+ if( (sensor_inp()&0x02) == 0x02 ) {
+ led_out( 0x0 );
+ led_m_set( RUN );
+ pattern = 11;
+ cnt1 = 0;
+ }
+ break;
+
+ case 51:
+ /* Right lane change processing at 1st process */
+ led_m_set( LCHANGE );
+ led_out( 0x01 );
+ handle( 0 );
+ motor2( 0 ,0 );
+ pattern = 52;
+ cnt1 = 0;
+ break;
+
+ case 52:
+ /* Right lane change processing at 2nd process */
+ if( cnt1 > 300 ) {
+ pattern = 53;
+ cnt1 = 0;
+ break;
+ }
+ break;
+
+ case 53:
+ /* Right lane change processing at 3rd process */
+ if( ( center_inp() == 0 ) && ( (sensor_inp()&0x0f) == 0x00 ) ) {
+ handle( 20 );
+ motor2( 30 ,diff(30) );
+ pattern = 54;
+ cnt1 = 0;
+ break;
+ }
+ switch( (sensor_inp()&0x0f) ) {
+ case 0x00:
+ /* Center -> straight */
+ handle( 0 );
+ motor2( 30 ,30 );
+ break;
+ case 0x02:
+ /* Left of center -> turn to right */
+ handle( 5 );
+ motor2( 30 ,diff(30) );
+ break;
+ case 0x04:
+ /* Right of center -> turn to left */
+ handle( -5 );
+ motor2( diff(30) ,30 );
+ break;
+ default:
+ break;
+ }
+ break;
+
+ case 54:
+ /* Right lane change clearing processing - wait until stable */
+ if( cnt1 > 800 ){
+ pattern = 55;
+ cnt1 = 0;
+ }
+ break;
+
+ case 55:
+ /* Right lane change end check */
+ if( (sensor_inp()&0x04) == 0x04 ) {
+ led_m_set( RUN );
+ led_out( 0x0 );
+ pattern = 11;
+ cnt1 = 0;
+ }
+ break;
+
+ default:
+ break;
+ }
+ }
+}
+
+//******************************************************************//
+// Initialize functions
+//*******************************************************************/
+//------------------------------------------------------------------//
+//Initialize MTU2 PWM functions
+//------------------------------------------------------------------//
+//MTU2_3, MTU2_4
+//Reset-Synchronized PWM mode
+//TIOC4A(P4_4) :Left-motor
+//TIOC4B(P4_5) :Right-motor
+//------------------------------------------------------------------//
+void init_MTU2_PWM_Motor( void )
+{
+ /* Port setting for S/W I/O Control */
+ /* alternative mode */
+
+ /* MTU2_4 (P4_4)(P4_5) */
+ GPIOPBDC4 = 0x0000; /* Bidirection mode disabled*/
+ GPIOPFCAE4 &= 0xffcf; /* The alternative function of a pin */
+ GPIOPFCE4 |= 0x0030; /* The alternative function of a pin */
+ GPIOPFC4 &= 0xffcf; /* The alternative function of a pin */
+ /* 2nd altemative function/output */
+ GPIOP4 &= 0xffcf; /* */
+ GPIOPM4 &= 0xffcf; /* p4_4,P4_5:output */
+ GPIOPMC4 |= 0x0030; /* P4_4,P4_5:double */
+
+ /* Module stop 33(MTU2) canceling */
+ CPGSTBCR3 &= 0xf7;
+
+ /* MTU2_3 and MTU2_4 (Motor PWM) */
+ MTU2TCR_3 = 0x20; /* TCNT Clear(TGRA), P0φ/1 */
+ MTU2TOCR1 = 0x04; /* */
+ MTU2TOCR2 = 0x40; /* N L>H P H>L */
+ MTU2TMDR_3 = 0x38; /* Buff:ON Reset-Synchronized PWM mode */
+ MTU2TMDR_4 = 0x30; /* Buff:ON */
+ MTU2TOER = 0xc6; /* TIOC3B,4A,4B enabled output */
+ MTU2TCNT_3 = MTU2TCNT_4 = 0; /* TCNT3,TCNT4 Set 0 */
+ MTU2TGRA_3 = MTU2TGRC_3 = MOTOR_PWM_CYCLE;
+ /* PWM-Cycle(1ms) */
+ MTU2TGRA_4 = MTU2TGRC_4 = 0; /* Left-motor(P4_4) */
+ MTU2TGRB_4 = MTU2TGRD_4 = 0; /* Right-motor(P4_5) */
+ MTU2TSTR |= 0x40; /* TCNT_4 Start */
+}
+
+//------------------------------------------------------------------//
+//Initialize MTU2 PWM functions
+//------------------------------------------------------------------//
+//MTU2_0
+//PWM mode 1
+//TIOC0A(P4_0) :Servo-motor
+//------------------------------------------------------------------//
+void init_MTU2_PWM_Servo( void )
+{
+ /* Port setting for S/W I/O Control */
+ /* alternative mode */
+
+ /* MTU2_0 (P4_0) */
+ GPIOPBDC4 = 0x0000; /* Bidirection mode disabled*/
+ GPIOPFCAE4 &= 0xfffe; /* The alternative function of a pin */
+ GPIOPFCE4 &= 0xfffe; /* The alternative function of a pin */
+ GPIOPFC4 |= 0x0001; /* The alternative function of a pin */
+ /* 2nd alternative function/output */
+ GPIOP4 &= 0xfffe; /* */
+ GPIOPM4 &= 0xfffe; /* p4_0:output */
+ GPIOPMC4 |= 0x0001; /* P4_0:double */
+
+ /* Module stop 33(MTU2) canceling */
+ CPGSTBCR3 &= 0xf7;
+
+ /* MTU2_0 (Motor PWM) */
+ MTU2TCR_0 = 0x22; /* TCNT Clear(TGRA), P0φ/16 */
+ MTU2TIORH_0 = 0x52; /* TGRA L>H, TGRB H>L */
+ MTU2TMDR_0 = 0x32; /* TGRC and TGRD = Buff-mode*/
+ /* PWM-mode1 */
+ MTU2TCNT_0 = 0; /* TCNT0 Set 0 */
+ MTU2TGRA_0 = MTU2TGRC_0 = SERVO_PWM_CYCLE;
+ /* PWM-Cycle(16ms) */
+ MTU2TGRB_0 = MTU2TGRD_0 = 0; /* Servo-motor(P4_0) */
+ MTU2TSTR |= 0x01; /* TCNT_0 Start */
+}
+
+//------------------------------------------------------------------//
+//Initialize Camera function
+//------------------------------------------------------------------//
+void init_Camera( void )
+{
+ /* NTSC-Video */
+ DisplayBase::graphics_error_t error;
+
+ /* Graphics initialization process */
+ error = Display.Graphics_init(NULL);
+ if (error != DisplayBase::GRAPHICS_OK) {
+ pc.printf("Line %d, error %d\n", __LINE__, error);
+ while (1);
+ }
+
+ error = Display.Graphics_Video_init( DisplayBase::INPUT_SEL_VDEC, NULL);
+ if( error != DisplayBase::GRAPHICS_OK ) {
+ while(1);
+ }
+
+ /* Interrupt callback function setting (Vsync signal input to scaler 0) */
+ error = Display.Graphics_Irq_Handler_Set(DisplayBase::INT_TYPE_S0_VI_VSYNC, 0, IntCallbackFunc_Vsync);
+ if (error != DisplayBase::GRAPHICS_OK) {
+ pc.printf("Line %d, error %d\n", __LINE__, error);
+ while (1);
+ }
+
+ /* Video capture setting (progressive form fixed) */
+ error = Display.Video_Write_Setting(
+ VIDEO_INPUT_CH,
+ DisplayBase::COL_SYS_NTSC_358,
+ write_buff_addr,
+ VIDEO_BUFFER_STRIDE,
+ DisplayBase::VIDEO_FORMAT_YCBCR422,
+ DisplayBase::WR_RD_WRSWA_32_16BIT,
+ PIXEL_VW,
+ PIXEL_HW
+ );
+ if (error != DisplayBase::GRAPHICS_OK) {
+ pc.printf("Line %d, error %d\n", __LINE__, error);
+ while (1);
+ }
+
+ /* Interrupt callback function setting (Field end signal for recording function in scaler 0) */
+ error = Display.Graphics_Irq_Handler_Set(VIDEO_INT_TYPE, 0, IntCallbackFunc_Vfield);
+ if (error != DisplayBase::GRAPHICS_OK) {
+ pc.printf("Line %d, error %d\n", __LINE__, error);
+ while (1);
+ }
+
+ /* Video write process start */
+ error = Display.Video_Start (VIDEO_INPUT_CH);
+ if (error != DisplayBase::GRAPHICS_OK) {
+ pc.printf("Line %d, error %d\n", __LINE__, error);
+ while (1);
+ }
+
+ /* Video write process stop */
+ error = Display.Video_Stop (VIDEO_INPUT_CH);
+ if (error != DisplayBase::GRAPHICS_OK) {
+ pc.printf("Line %d, error %d\n", __LINE__, error);
+ while (1);
+ }
+
+ /* Video write process start */
+ error = Display.Video_Start (VIDEO_INPUT_CH);
+ if (error != DisplayBase::GRAPHICS_OK) {
+ pc.printf("Line %d, error %d\n", __LINE__, error);
+ while (1);
+ }
+
+ /* Wait vsync to update resister */
+ WaitVsync(1);
+
+ /* Wait 2 Vfield(Top or bottom field) */
+ WaitVfield(2);
+}
+
+//------------------------------------------------------------------//
+//ChangeFrameBuffer function
+//------------------------------------------------------------------//
+void ChangeFrameBuffer( void )
+{
+ /* NTSC-Video */
+ DisplayBase::graphics_error_t error;
+
+ /* Change write buffer */
+ if (write_buff_addr == FrameBuffer_Video_A) {
+ write_buff_addr = FrameBuffer_Video_B;
+ save_buff_addr = FrameBuffer_Video_A;
+ } else {
+ write_buff_addr = FrameBuffer_Video_A;
+ save_buff_addr = FrameBuffer_Video_B;
+ }
+ error = Display.Video_Write_Change(
+ VIDEO_INPUT_CH,
+ write_buff_addr,
+ VIDEO_BUFFER_STRIDE);
+ if (error != DisplayBase::GRAPHICS_OK) {
+ pc.printf("Line %d, error %d\n", __LINE__, error);
+ while (1);
+ }
+}
+
+//------------------------------------------------------------------//
+// @brief Interrupt callback function
+// @param[in] int_type : VDC5 interrupt type
+// @retval None
+//------------------------------------------------------------------//
+static void IntCallbackFunc_Vfield(DisplayBase::int_type_t int_type)
+{
+ if (vfield_count > 0) {
+ vfield_count--;
+ }
+ /* top or bottom (Change) */
+ if( vfield_count2 == 0 ) vfield_count2 = 1;
+ else vfield_count2 = 0;
+ led_m_user( vfield_count2 );
+}
+
+//------------------------------------------------------------------//
+// @brief Wait for the specified number of times Vsync occurs
+// @param[in] wait_count : Wait count
+// @retval None
+//------------------------------------------------------------------//
+static void WaitVfield(const int32_t wait_count)
+{
+ vfield_count = wait_count;
+ while (vfield_count > 0) {
+ /* Do nothing */
+ }
+}
+
+//------------------------------------------------------------------//
+// @brief Interrupt callback function for Vsync interruption
+// @param[in] int_type : VDC5 interrupt type
+// @retval None
+//------------------------------------------------------------------//
+static void IntCallbackFunc_Vsync(DisplayBase::int_type_t int_type)
+{
+ if (vsync_count > 0) {
+ vsync_count--;
+ }
+}
+
+//------------------------------------------------------------------//
+// @brief Wait for the specified number of times Vsync occurs
+// @param[in] wait_count : Wait count
+// @retval None
+//------------------------------------------------------------------//
+static void WaitVsync(const int32_t wait_count)
+{
+ vsync_count = wait_count;
+ while (vsync_count > 0) {
+ /* Do nothing */
+ }
+}
+
+//------------------------------------------------------------------//
+// Interrupt function( intTimer )
+//------------------------------------------------------------------//
+void intTimer( void )
+{
+ static int counter = 0; /* Only variable for image process */
+
+ cnt0++;
+ cnt1++;
+
+ /* field check */
+ if( vfield_count2 != vfield_count2_buff ) {
+ vfield_count2_buff = vfield_count2;
+ counter = 0;
+ }
+
+ /* Top field / bottom field */
+ switch( counter++ ) {
+ case 0:
+ ImageCopy( write_buff_addr, PIXEL_HW, PIXEL_VW, ImageData_A, vfield_count2 );
+ break;
+ case 1:
+ ImageCopy( write_buff_addr, PIXEL_HW, PIXEL_VW, ImageData_A, vfield_count2 );
+ break;
+ case 2:
+ Extraction_Brightness( ImageData_A, PIXEL_HW, PIXEL_VW, ImageData_B, vfield_count2 );
+ break;
+ case 3:
+ Extraction_Brightness( ImageData_A, PIXEL_HW, PIXEL_VW, ImageData_B, vfield_count2 );
+ break;
+ case 4:
+ ImageReduction( ImageData_B, PIXEL_HW, PIXEL_VW, ImageComp_B, Rate );
+ break;
+ case 5:
+ ImageReduction( ImageData_B, PIXEL_HW, PIXEL_VW, ImageComp_B, Rate );
+ break;
+ case 6:
+ Binarization( ImageComp_B, (PIXEL_HW * Rate), (PIXEL_VW * Rate), ImageBinary, threshold_buff );
+ if( !initFlag ) SenVal1 = sensor_process( ImageBinary, (PIXEL_HW * Rate), (PIXEL_VW * Rate), Sen1Px, 12 );
+ break;
+ case 7:
+ if( !initFlag ) PatternMatching_process( ImageBinary, (PIXEL_HW * Rate), (PIXEL_VW * Rate), &RightCrank, 2, 10, 2, 8 );
+ break;
+ case 8:
+ if( !initFlag ) PatternMatching_process( ImageBinary, (PIXEL_HW * Rate), (PIXEL_VW * Rate), &RightLaneChange, 4, 6, 1, 3 );
+ break;
+ default:
+ break;
+ }
+
+ /* LED(rgb) on the GR-peach board */
+ led_m_process();
+}
+
+//******************************************************************//
+// functions ( on GR-PEACH board )
+//*******************************************************************/
+//------------------------------------------------------------------//
+//led_rgb Function
+//------------------------------------------------------------------//
+void led_rgb(int led)
+{
+ LED_R = led & 0x1;
+ LED_G = (led >> 1 ) & 0x1;
+ LED_B = (led >> 2 ) & 0x1;
+}
+
+//------------------------------------------------------------------//
+//user_button_get Function
+//------------------------------------------------------------------//
+unsigned char user_button_get( void )
+{
+ return (~user_botton) & 0x1; /* Read ports with switches */
+}
+
+//------------------------------------------------------------------//
+//led_m_user Function
+//------------------------------------------------------------------//
+void led_m_user( int led )
+{
+ USER_LED = led & 0x01;
+}
+
+//------------------------------------------------------------------//
+//led_m_set Function
+//------------------------------------------------------------------//
+void led_m_set( int set )
+{
+ led_pattern = set;
+}
+
+//------------------------------------------------------------------//
+//led_m_process Function for only interrupt
+//------------------------------------------------------------------//
+void led_m_process( void )
+{
+ int led_color;
+ int onTime;
+ int offTime;
+ static unsigned long cnt_led_m;
+
+ switch( led_pattern ) {
+ case RUN:
+ led_color = LED_GREEN;
+ onTime = 500;
+ offTime = 500;
+ break;
+ case STOP:
+ led_color = LED_RED;
+ onTime = 500;
+ offTime = 0;
+ break;
+ case ERROR:
+ led_color = LED_RED;
+ onTime = 100;
+ offTime = 100;
+ break;
+ case DEBUG:
+ led_color = LED_BLUE;
+ onTime = 50;
+ offTime = 50;
+ break;
+ case CRANK:
+ led_color = LED_YELLOW;
+ onTime = 500;
+ offTime = 500;
+ break;
+ case LCHANGE:
+ led_color = LED_BLUE;
+ onTime = 500;
+ offTime = 500;
+ break;
+ default:
+ led_color = LED_OFF;
+ onTime = 500;
+ offTime = 500;
+ break;
+ }
+
+ cnt_led_m++;
+
+ /* Display */
+ if( cnt_led_m < onTime ) led_rgb( led_color );
+ else if( cnt_led_m < ( onTime + offTime ) ) led_rgb( LED_OFF );
+ else cnt_led_m = 0;
+}
+
+//******************************************************************//
+// functions ( on Motor drive board )
+//*******************************************************************/
+//------------------------------------------------------------------//
+//led_out Function
+//------------------------------------------------------------------//
+void led_out(int led)
+{
+ led = ~led;
+ LED_3 = led & 0x1;
+ LED_2 = ( led >> 1 ) & 0x1;
+}
+
+//------------------------------------------------------------------//
+//pushsw_get Function
+//------------------------------------------------------------------//
+unsigned char pushsw_get( void )
+{
+ return (~push_sw) & 0x1; /* Read ports with switches */
+}
+
+//------------------------------------------------------------------//
+//motor speed control(PWM)
+//Arguments: motor:-100 to 100
+//Here, 0 is stop, 100 is forward, -100 is reverse
+//------------------------------------------------------------------//
+void motor( int accele_l, int accele_r )
+{
+ int sw_data;
+
+ sw_data = dipsw_get() + 5;
+ accele_l = ( accele_l * sw_data ) / 20;
+ accele_r = ( accele_r * sw_data ) / 20;
+
+ /* Left Motor Control */
+ if( accele_l >= 0 ) {
+ /* forward */
+ Left_motor_signal = 0;
+ MTU2TGRC_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * accele_l / 100;
+ } else {
+ /* reverse */
+ Left_motor_signal = 1;
+ MTU2TGRC_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * ( -accele_l ) / 100;
+ }
+
+ /* Right Motor Control */
+ if( accele_r >= 0 ) {
+ /* forward */
+ Right_motor_signal = 0;
+ MTU2TGRD_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * accele_r / 100;
+ } else {
+ /* reverse */
+ Right_motor_signal = 1;
+ MTU2TGRD_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * ( -accele_r ) / 100;
+ }
+}
+
+//------------------------------------------------------------------//
+//motor speed control(PWM)
+//Arguments: motor:-100 to 100
+//Here, 0 is stop, 100 is forward, -100 is reverse
+//------------------------------------------------------------------//
+void motor2( int accele_l, int accele_r )
+{
+ /* Left Motor Control */
+ if( accele_l >= 0 ) {
+ /* forward */
+ Left_motor_signal = 0;
+ MTU2TGRC_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * accele_l / 100;
+ } else {
+ /* reverse */
+ Left_motor_signal = 1;
+ MTU2TGRC_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * ( -accele_l ) / 100;
+ }
+
+ /* Right Motor Control */
+ if( accele_r >= 0 ) {
+ /* forward */
+ Right_motor_signal = 0;
+ MTU2TGRD_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * accele_r / 100;
+ } else {
+ /* reverse */
+ Right_motor_signal = 1;
+ MTU2TGRD_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * ( -accele_r ) / 100;
+ }
+}
+
+//------------------------------------------------------------------//
+//handle Function
+//------------------------------------------------------------------//
+void handle( int angle )
+{
+ handle_buff = angle;
+ /* When the servo move from left to right in reverse, replace "-" with "+" */
+ MTU2TGRD_0 = SERVO_CENTER - angle * HANDLE_STEP;
+}
+
+//------------------------------------------------------------------//
+//diff Function
+//------------------------------------------------------------------//
+int diff( int pwm )
+{
+ int i, ret;
+
+ i = handle_buff;
+ if( i < 0 ) i = -i;
+ if( i > 45 ) i = 45;
+ ret = revolution_difference[i] * pwm / 100;
+
+ return ret;
+}
+
+//******************************************************************//
+// functions ( on Shield board )
+//*******************************************************************/
+//------------------------------------------------------------------//
+//Dipsw get Function
+//------------------------------------------------------------------//
+unsigned char dipsw_get( void )
+{
+ return( dipsw.read() & 0x0f );
+}
+
+//******************************************************************//
+// digital sensor functions
+//*******************************************************************/
+//------------------------------------------------------------------//
+// init sensor Function
+//------------------------------------------------------------------//
+int init_sensor( unsigned char *BuffAddrIn, int HW, int VW, int Cx, int *SENPx, int Y )
+{
+ int X;
+ int L1x, R1x;
+ int i, error_cnt;
+
+ //Left side
+ for( X = Cx; X > 1; X-- ){
+ if( BuffAddrIn[ ( Y * HW ) + ( X - 0 ) ] == 0 && BuffAddrIn[ ( Y * HW ) + ( X - 1 ) ] == 0 ) {
+ L1x = X;
+ break;
+ }
+ }
+ //Right side
+ for( X = Cx; X < ( HW - 1 ); X++ ){
+ if( BuffAddrIn[ ( Y * HW ) + ( X + 0 ) ] == 0 && BuffAddrIn[ ( Y * HW ) + ( X + 1 ) ] == 0 ) {
+ R1x = X;
+ break;
+ }
+ }
+
+ SENPx[ 4 ] = L1x + ( ( R1x - L1x ) / 2 ); // Center
+ SENPx[ 2 ] = L1x; // L1
+ SENPx[ 1 ] = R1x; // R1
+ SENPx[ 3 ] = SENPx[ 2 ] - ( SENPx[ 4 ] - SENPx[ 2 ] ); //L2
+ SENPx[ 0 ] = SENPx[ 1 ] + ( SENPx[ 1 ] - SENPx[ 4 ] ); //R2
+
+ /* error check */
+ error_cnt = 0;
+ for( i = 0; i < 4; i++ ){
+ if( SENPx[4] == SENPx[i] ) error_cnt += 1;
+ }
+
+//#define DEBUG_MODE
+#ifdef DEBUG_MODE
+ pc.printf( "L2=%2d, L1=%2d, Cx=%2d, R1=%2d, R2=%2d\n\r", SENPx[ 3 ], SENPx[ 2 ], SENPx[ 4 ], SENPx[ 1 ], SENPx[ 0 ] );
+ if( error_cnt != 0 ) pc.printf( "init_sensor function Error %1d\n\r", error_cnt );
+ else pc.printf( "init_sensor function complete\n\r" );
+ pc.printf( "\n\r" );
+#endif
+
+ return error_cnt;
+}
+
+//------------------------------------------------------------------//
+//sensor_process Function(Interrupt function)
+//------------------------------------------------------------------//
+unsigned char sensor_process( unsigned char *BuffAddrIn, int HW, int VW, int *SENPx, int Y )
+{
+ int sensor;
+ int data;
+
+ sensor = 0x00;
+ if( VW < Y ) {
+ pc.printf( "sensor process function error\n\r" );
+ return sensor;
+ }
+
+ data = BuffAddrIn[ ( Y * HW ) + SENPx[ 4 ] ] & 0x01;
+ sensor |= ( data << 4 ) & 0x10;
+ data = BuffAddrIn[ ( Y * HW ) + SENPx[ 3 ] ] & 0x01;
+ sensor |= ( data << 3 ) & 0x08;
+ data = BuffAddrIn[ ( Y * HW ) + SENPx[ 2 ] ] & 0x01;
+ sensor |= ( data << 2 ) & 0x04;
+ data = BuffAddrIn[ ( Y * HW ) + SENPx[ 1 ] ] & 0x01;
+ sensor |= ( data << 1 ) & 0x02;
+ sensor |= BuffAddrIn[ ( Y * HW ) + SENPx[ 0 ] ] & 0x01;
+
+ sensor &= 0x1f;
+
+ return sensor;
+}
+
+//------------------------------------------------------------------//
+//sensor Function
+//------------------------------------------------------------------//
+unsigned char sensor_inp( void )
+{
+ return SenVal1 & 0x0f;
+}
+
+//------------------------------------------------------------------//
+//sensor Function
+//------------------------------------------------------------------//
+unsigned char center_inp( void )
+{
+ return ( SenVal1 >> 4 ) & 0x01;
+}
+
+//------------------------------------------------------------------//
+// RightCrank Check
+// Return values: 0: no triangle mark, 1: Triangle mark
+//------------------------------------------------------------------//
+int RightCrankCheck( int percentage )
+{
+ int ret;
+
+ ret = 0;
+ if( RightCrank.p >= percentage ) {
+ ret = 1;
+ }
+
+ return ret;
+}
+
+//------------------------------------------------------------------//
+// RightLaneChange Check
+// Return values: 0: no triangle mark, 1: Triangle mark
+//------------------------------------------------------------------//
+int RightLaneChangeCheck( int percentage )
+{
+ int ret;
+
+ ret = 0;
+ if( RightLaneChange.p >= percentage ) {
+ ret = 1;
+ }
+
+ return ret;
+}
+
+//******************************************************************//
+// Debug functions
+//*******************************************************************/
+//------------------------------------------------------------------//
+//Image Data Output( for the Excel )
+//------------------------------------------------------------------//
+void ImageData_Serial_Out1( unsigned char *ImageData, int HW, int VW )
+{
+ int Xp, Yp, inc;
+
+ for( Yp = 0, inc = 0; Yp < VW; Yp++ ) {
+ for( Xp = 0; Xp < HW; Xp++, inc++ ) {
+ pc.printf( "%d,", ImageData[ inc ] );
+ }
+ pc.printf("\n\r");
+ }
+}
+
+//------------------------------------------------------------------//
+//Image Data Output2( for TeraTerm )
+//------------------------------------------------------------------//
+void ImageData_Serial_Out2( unsigned char *ImageData, int HW, int VW )
+{
+ int Xp, Yp;
+ int i;
+
+ for( Yp = 0; Yp < VW; Yp++ ) {
+ for( Xp = 0; Xp < HW; Xp++ ) {
+ pc.printf( "%d ", ImageData[Xp + (Yp * HW)] );
+ }
+ pc.printf( "\n\r" );
+ }
+
+ //Add display
+ pc.printf( "\n\r" );
+ pc.printf( "sensor_inp = 0x%02x\n\r", sensor_inp() );
+ pc.printf( "center_inp = 0x%02x\n\r", center_inp() );
+ pc.printf( "RightCrank = %01d, = %3d%%, X = %2d, Y = %2d\n\r", RightCrankCheck(80), RightCrank.p, RightCrank.x, RightCrank.y );
+ pc.printf( "RightLaneChange = %01d, = %3d%%, X = %2d, Y = %2d\n\r", RightLaneChangeCheck(80), RightLaneChange.p, RightLaneChange.x, RightLaneChange.y );
+ pc.printf( "\n\r" );
+ VW += 6;
+
+ pc.printf( "Threshold Check ( please user button on GR-peach board )\n\r" );
+ pc.printf( "\n\r" );
+ VW += 2;
+
+ if( user_button_get() ){
+ pc.printf( "Please Threshold\n\r" );
+ pc.printf( " = " );
+ pc.scanf( "%d", &i );
+ pc.printf( "\n\r" );
+ threshold_buff = i;
+ VW += 2;
+ }
+
+ pc.printf( "\033[%dA" , VW );
+}
+
+//------------------------------------------------------------------//
+//Image Data Output3( for TeraTerm )
+//------------------------------------------------------------------//
+void ImageData_Serial_Out3( unsigned char *ImageData, int HW, int VW, int color_pattern )
+{
+ int X, Y;
+ int Px, Py;
+ int HW_T;//HW Twice
+ int value;
+
+ HW_T = HW + HW;
+
+ /* Camera module test process 2 */
+ pc.printf( "//,X-Size,Y-Size" );
+ pc.printf( "\n\r" );
+ pc.printf( "#SIZE,%3d,%3d", HW, VW );
+ pc.printf( "\n\r" );
+ pc.printf( "//,X-Point,Y-Point" );
+ pc.printf( "\n\r" );
+
+ switch( color_pattern ) {
+ case COLOR:
+ //YCBCR_422 Color
+ for( Py = 0, Y = 0; Py < VW; Py+=1, Y++ ){
+ for( Px = 0, X = 0; Px < HW_T; Px+=4, X+=2 ){
+ pc.printf( "#YCbCr," );
+ /*Xp*/pc.printf( "%d,", X);
+ /*Yp*/pc.printf( "%d,", Y);
+ /*Y0*/pc.printf( "%d,", ImageData[ ( Px + 0 ) + ( HW_T * Py ) ] );//6
+ /*Cb*/pc.printf( "%d,", ImageData[ ( Px + 1 ) + ( HW_T * Py ) ] );//5
+ /*Cr*/pc.printf( "%d,", ImageData[ ( Px + 3 ) + ( HW_T * Py ) ] );//7
+ pc.printf( "\n\r" );
+
+ pc.printf( "#YCbCr," );
+ /*Xp*/pc.printf( "%d,", X+1);
+ /*Yp*/pc.printf( "%d,", Y);
+ /*Y1*/pc.printf( "%d,", ImageData[ ( Px + 2 ) + ( HW_T * Py ) ] );//4
+ /*Cb*/pc.printf( "%d,", ImageData[ ( Px + 1 ) + ( HW_T * Py ) ] );//5
+ /*Cr*/pc.printf( "%d,", ImageData[ ( Px + 3 ) + ( HW_T * Py ) ] );//7
+ pc.printf( "\n\r" );
+ }
+ }
+ break;
+ case GREY_SCALE:
+ //YCBCR_422 GreyScale
+ for( Y = 0; Y < VW; Y++ ){
+ for( X = 0; X < HW; X+=2 ){
+ pc.printf( "#YCbCr," );
+ /*Xp*/pc.printf( "%d,", X);
+ /*Yp*/pc.printf( "%d,", Y);
+ /*Y0*/pc.printf( "%d,", ImageData[ ( Y * HW ) + ( X + 0 ) ] );//6
+ /*Cb*/pc.printf( "%d,", 128);//5
+ /*Cr*/pc.printf( "%d,", 128);//7
+ pc.printf( "\n\r" );
+
+ pc.printf( "#YCbCr," );
+ /*Xp*/pc.printf( "%d,", X+1);
+ /*Yp*/pc.printf( "%d,", Y);
+ /*Y1*/pc.printf( "%d,", ImageData[ ( Y * HW ) + ( X + 1 ) ] );//4
+ /*Cb*/pc.printf( "%d,", 128);//5
+ /*Cr*/pc.printf( "%d,", 128);//7
+ pc.printf( "\n\r" );
+ }
+ }
+ break;
+ case BINARY:
+ //YCBCR_422 Binary
+ for( Y = 0; Y < VW; Y++ ){
+ for( X = 0; X < HW; X+=2 ){
+ pc.printf( "#YCbCr," );
+ /*Xp*/pc.printf( "%d,", X);
+ /*Yp*/pc.printf( "%d,", Y);
+ value = ImageData[ ( Y * HW ) + ( X + 0 ) ];
+ if( value ) value = 255;
+ else value = 0;
+ /*Y0*/pc.printf( "%d,", value );//6
+ /*Cb*/pc.printf( "%d,", 128);//5
+ /*Cr*/pc.printf( "%d,", 128);//7
+ pc.printf( "\n\r" );
+
+ pc.printf( "#YCbCr," );
+ /*Xp*/pc.printf( "%d,", X+1);
+ /*Yp*/pc.printf( "%d,", Y);
+ value = ImageData[ ( Y * HW ) + ( X + 1 ) ];
+ if( value ) value = 255;
+ else value = 0;
+ /*Y1*/pc.printf( "%d,", value );//4
+ /*Cb*/pc.printf( "%d,", 128);//5
+ /*Cr*/pc.printf( "%d,", 128);//7
+ pc.printf( "\n\r" );
+ }
+ }
+ break;
+ default:
+ break;
+ }
+ pc.printf( "End\n\r" );
+}
+
+//------------------------------------------------------------------//
+// End of file
+//------------------------------------------------------------------//
\ No newline at end of file
diff -r 000000000000 -r 00b6f7454ada mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue Oct 30 09:06:30 2018 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/abea610beb85 \ No newline at end of file