Micon Car Rally / Mbed 2 deprecated TraceMark_Program_60fps

This program is Trace and Mark detection Program by Image Processing(NTSC 60fps).

Dependencies:   GR-PEACH_video mbed

main.cpp@0:158dbfb66e62, 2016-09-22 (annotated)

Committer:
TetsuyaKonno
Date:
Thu Sep 22 09:27:51 2016 +0000
Revision:
0:158dbfb66e62
Child:
1:4a94c87a7d04
First program file

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TetsuyaKonno 0:158dbfb66e62 1 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 2 //Supported MCU: RZ/A1H
TetsuyaKonno 0:158dbfb66e62 3 //File Contents: Trace Program by Image Processing
TetsuyaKonno 0:158dbfb66e62 4 // (GR-PEACH version on the Micon Car)
TetsuyaKonno 0:158dbfb66e62 5 //Version number: Ver.1.00
TetsuyaKonno 0:158dbfb66e62 6 //Date: 2016.09.22
TetsuyaKonno 0:158dbfb66e62 7 //Copyright: Renesas Electronics Corporation
TetsuyaKonno 0:158dbfb66e62 8 // Hitachi Document Solutions Co., Ltd.
TetsuyaKonno 0:158dbfb66e62 9 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 10
TetsuyaKonno 0:158dbfb66e62 11 //This program supports the following boards:
TetsuyaKonno 0:158dbfb66e62 12 //* GR-PEACH(E version)
TetsuyaKonno 0:158dbfb66e62 13 //* Motor drive board Ver.5
TetsuyaKonno 0:158dbfb66e62 14 //* Camera module (SC-310)
TetsuyaKonno 0:158dbfb66e62 15
TetsuyaKonno 0:158dbfb66e62 16 //Include
TetsuyaKonno 0:158dbfb66e62 17 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 18 #include "mbed.h"
TetsuyaKonno 0:158dbfb66e62 19 #include "math.h"
TetsuyaKonno 0:158dbfb66e62 20 #include "iodefine.h"
TetsuyaKonno 0:158dbfb66e62 21 #include "DisplayBace.h"
TetsuyaKonno 0:158dbfb66e62 22
TetsuyaKonno 0:158dbfb66e62 23 //Define
TetsuyaKonno 0:158dbfb66e62 24 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 25 //Motor PWM cycle
TetsuyaKonno 0:158dbfb66e62 26 #define MOTOR_PWM_CYCLE 33332 /* Motor PWM period */
TetsuyaKonno 0:158dbfb66e62 27 /* 1ms P0φ/1 = 0.03us */
TetsuyaKonno 0:158dbfb66e62 28 //Motor speed
TetsuyaKonno 0:158dbfb66e62 29 #define MAX_SPEED 40 /* motor() set: 0 to 100 */
TetsuyaKonno 0:158dbfb66e62 30
TetsuyaKonno 0:158dbfb66e62 31 //Servo PWM cycle
TetsuyaKonno 0:158dbfb66e62 32 #define SERVO_PWM_CYCLE 33332 /* SERVO PWM period */
TetsuyaKonno 0:158dbfb66e62 33 /* 16ms P0φ/16 = 0.48us */
TetsuyaKonno 0:158dbfb66e62 34 #define SERVO_CENTER 3124 /* 1.5ms / 0.48us - 1 = 3124*/
TetsuyaKonno 0:158dbfb66e62 35 #define HANDLE_STEP 18 /* 1 degree value */
TetsuyaKonno 0:158dbfb66e62 36
TetsuyaKonno 0:158dbfb66e62 37 //LED Color on GR-PEACH
TetsuyaKonno 0:158dbfb66e62 38 #define LED_OFF 0x00
TetsuyaKonno 0:158dbfb66e62 39 #define LED_RED 0x01
TetsuyaKonno 0:158dbfb66e62 40 #define LED_GREEN 0x02
TetsuyaKonno 0:158dbfb66e62 41 #define LED_YELLOW 0x03
TetsuyaKonno 0:158dbfb66e62 42 #define LED_BLUE 0x04
TetsuyaKonno 0:158dbfb66e62 43 #define LED_PURPLE 0x05
TetsuyaKonno 0:158dbfb66e62 44 #define LED_SKYBLUE 0x06
TetsuyaKonno 0:158dbfb66e62 45 #define LED_WHITE 0x07
TetsuyaKonno 0:158dbfb66e62 46
TetsuyaKonno 0:158dbfb66e62 47 //Status
TetsuyaKonno 0:158dbfb66e62 48 #define ERROR 0x00
TetsuyaKonno 0:158dbfb66e62 49 #define STOP 0x01
TetsuyaKonno 0:158dbfb66e62 50 #define RUN 0x02
TetsuyaKonno 0:158dbfb66e62 51 #define DEBUG 0x03
TetsuyaKonno 0:158dbfb66e62 52 #define MOTOR_START 0x04
TetsuyaKonno 0:158dbfb66e62 53 #define MOTOR_STOP 0x05
TetsuyaKonno 0:158dbfb66e62 54 #define MARK_T 0x06
TetsuyaKonno 0:158dbfb66e62 55
TetsuyaKonno 0:158dbfb66e62 56 //Define(NTSC-Video)
TetsuyaKonno 0:158dbfb66e62 57 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 58 #define VIDEO_INPUT_CH (DisplayBase::VIDEO_INPUT_CHANNEL_0)
TetsuyaKonno 0:158dbfb66e62 59 #define VIDEO_INT_TYPE (DisplayBase::INT_TYPE_S0_VFIELD)
TetsuyaKonno 0:158dbfb66e62 60 #define DATA_SIZE_PER_PIC (2u)
TetsuyaKonno 0:158dbfb66e62 61
TetsuyaKonno 0:158dbfb66e62 62 /*! Frame buffer stride: Frame buffer stride should be set to a multiple of 32 or 128
TetsuyaKonno 0:158dbfb66e62 63 in accordance with the frame buffer burst transfer mode. */
TetsuyaKonno 0:158dbfb66e62 64 #define PIXEL_HW (320u) /* QVGA */
TetsuyaKonno 0:158dbfb66e62 65 #define PIXEL_VW (240u) /* QVGA */
TetsuyaKonno 0:158dbfb66e62 66 #define VIDEO_BUFFER_STRIDE (((PIXEL_HW * DATA_SIZE_PER_PIC) + 31u) & ~31u)
TetsuyaKonno 0:158dbfb66e62 67 #define VIDEO_BUFFER_HEIGHT (PIXEL_VW)
TetsuyaKonno 0:158dbfb66e62 68
TetsuyaKonno 0:158dbfb66e62 69 //Constructor
TetsuyaKonno 0:158dbfb66e62 70 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 71 Ticker interrput;
TetsuyaKonno 0:158dbfb66e62 72 Serial pc(USBTX, USBRX);
TetsuyaKonno 0:158dbfb66e62 73 DigitalOut LED_R(P6_13); /* LED1 on the GR-PEACH board */
TetsuyaKonno 0:158dbfb66e62 74 DigitalOut LED_G(P6_14); /* LED2 on the GR-PEACH board */
TetsuyaKonno 0:158dbfb66e62 75 DigitalOut LED_B(P6_15); /* LED3 on the GR-PEACH board */
TetsuyaKonno 0:158dbfb66e62 76 DigitalOut USER_LED(P6_12); /* USER_LED on the GR-PEACH board */
TetsuyaKonno 0:158dbfb66e62 77 DigitalIn user_botton(P6_0); /* SW1 on the GR-PEACH board */
TetsuyaKonno 0:158dbfb66e62 78
TetsuyaKonno 0:158dbfb66e62 79 DigitalOut Left_motor_signal(P4_6); /* Used by motor fanction */
TetsuyaKonno 0:158dbfb66e62 80 DigitalOut Right_motor_signal(P4_7); /* Used by motor fanction */
TetsuyaKonno 0:158dbfb66e62 81 DigitalIn push_sw(P2_13); /* SW1 on the Motor Drive board */
TetsuyaKonno 0:158dbfb66e62 82 DigitalOut LED_3(P2_14); /* LED3 on the Motor Drive board */
TetsuyaKonno 0:158dbfb66e62 83 DigitalOut LED_2(P2_15); /* LED2 on the Motor Drive board */
TetsuyaKonno 0:158dbfb66e62 84
TetsuyaKonno 0:158dbfb66e62 85 //Prototype(NTSC-video)
TetsuyaKonno 0:158dbfb66e62 86 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 87 static void IntCallbackFunc_Vfield(DisplayBase::int_type_t int_type);
TetsuyaKonno 0:158dbfb66e62 88 static void WaitVfield(const int32_t wait_count);
TetsuyaKonno 0:158dbfb66e62 89 static void IntCallbackFunc_Vsync(DisplayBase::int_type_t int_type);
TetsuyaKonno 0:158dbfb66e62 90 static void WaitVsync(const int32_t wait_count);
TetsuyaKonno 0:158dbfb66e62 91
TetsuyaKonno 0:158dbfb66e62 92 //Prototype
TetsuyaKonno 0:158dbfb66e62 93 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 94 //Peripheral functions
TetsuyaKonno 0:158dbfb66e62 95 void init_MTU2_PWM_Motor( void ); /* Initialize PWM functions */
TetsuyaKonno 0:158dbfb66e62 96 void init_MTU2_PWM_Servo( void ); /* Initialize PWM functions */
TetsuyaKonno 0:158dbfb66e62 97 void intTimer( void ); /* Interrupt fanction */
TetsuyaKonno 0:158dbfb66e62 98
TetsuyaKonno 0:158dbfb66e62 99 //GR-peach board
TetsuyaKonno 0:158dbfb66e62 100 void led_rgb(int led);
TetsuyaKonno 0:158dbfb66e62 101 void led_m_user( int led );
TetsuyaKonno 0:158dbfb66e62 102 unsigned int user_button_get( void );
TetsuyaKonno 0:158dbfb66e62 103 void led_m_set( int set );
TetsuyaKonno 0:158dbfb66e62 104 void led_m_process( void ); /* Function for only interrupt */
TetsuyaKonno 0:158dbfb66e62 105
TetsuyaKonno 0:158dbfb66e62 106 //Motor drive board
TetsuyaKonno 0:158dbfb66e62 107 void led_out(int led);
TetsuyaKonno 0:158dbfb66e62 108 unsigned int pushsw_get( void );
TetsuyaKonno 0:158dbfb66e62 109 void motor( int accele_l, int accele_r );
TetsuyaKonno 0:158dbfb66e62 110 void handle( int angle );
TetsuyaKonno 0:158dbfb66e62 111 int diff( int pwm );
TetsuyaKonno 0:158dbfb66e62 112
TetsuyaKonno 0:158dbfb66e62 113 //Prototype(Image process)
TetsuyaKonno 0:158dbfb66e62 114 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 115 void Image_Extraction( unsigned char *buff_addr, unsigned char *Data_Y, int frame );
TetsuyaKonno 0:158dbfb66e62 116 void Image_Reduction( unsigned char *Data_Y, int Data_W , unsigned char *Comp_Y, int Comp_M );
TetsuyaKonno 0:158dbfb66e62 117 void Binarization_process( unsigned char *Comp_Y, unsigned char *Binary, long items, int threshold );
TetsuyaKonno 0:158dbfb66e62 118
TetsuyaKonno 0:158dbfb66e62 119 //Prototype(Digital sensor process)
TetsuyaKonno 0:158dbfb66e62 120 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 121 int CenterLine_Corrective( unsigned char *Binary );
TetsuyaKonno 0:158dbfb66e62 122 void digital_sensor_process( unsigned char *Binary ); /* Function for only interrupt */
TetsuyaKonno 0:158dbfb66e62 123 unsigned char digital_sensor( void );
TetsuyaKonno 0:158dbfb66e62 124
TetsuyaKonno 0:158dbfb66e62 125 //Prototype(Mark detection process)
TetsuyaKonno 0:158dbfb66e62 126 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 127 void Image_part_Extraction( unsigned char *Binary, int Width, int Xpix, int Ypix, unsigned char *Data_B, int x_size, int y_size );
TetsuyaKonno 0:158dbfb66e62 128 double Standard_Deviation( unsigned char *data, double *Devi, int items );
TetsuyaKonno 0:158dbfb66e62 129 double Covariance( double *Devi_A, double *Devi_B, int items );
TetsuyaKonno 0:158dbfb66e62 130 int Judgement_ImageMatching( double covari, double SDevi_A, double SDevi_B );
TetsuyaKonno 0:158dbfb66e62 131 void MarkDetect_process_T( void );
TetsuyaKonno 0:158dbfb66e62 132 int MarkCheck_Triangle( int percentage );
TetsuyaKonno 0:158dbfb66e62 133
TetsuyaKonno 0:158dbfb66e62 134 //Prototype(Display Debug)
TetsuyaKonno 0:158dbfb66e62 135 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 136 void ImageData_Serial_Out( unsigned char *Data_Y, int Width );
TetsuyaKonno 0:158dbfb66e62 137 void ImageData_Serial_Out2( unsigned char *Data_Y, int Width );
TetsuyaKonno 0:158dbfb66e62 138
TetsuyaKonno 0:158dbfb66e62 139 //Globle variable (NTSC-video)
TetsuyaKonno 0:158dbfb66e62 140 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 141 static uint8_t FrameBuffer_Video_A[VIDEO_BUFFER_STRIDE * VIDEO_BUFFER_HEIGHT]__attribute((section("NC_BSS"),aligned(16))); //16 bytes aligned!;
TetsuyaKonno 0:158dbfb66e62 142 uint8_t * write_buff_addr = FrameBuffer_Video_A;
TetsuyaKonno 0:158dbfb66e62 143 static volatile int32_t vsync_count;
TetsuyaKonno 0:158dbfb66e62 144 static volatile int32_t vfield_count;
TetsuyaKonno 0:158dbfb66e62 145 static volatile int32_t vfield_count2 = 1;
TetsuyaKonno 0:158dbfb66e62 146 static volatile int32_t vfield_count2_buff;
TetsuyaKonno 0:158dbfb66e62 147
TetsuyaKonno 0:158dbfb66e62 148 //Globle variable for Image process
TetsuyaKonno 0:158dbfb66e62 149 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 150 unsigned char ImageData_A[160*120];
TetsuyaKonno 0:158dbfb66e62 151 unsigned char ImageComp_A[20*15];
TetsuyaKonno 0:158dbfb66e62 152 unsigned char ImageBinary[20*15];
TetsuyaKonno 0:158dbfb66e62 153
TetsuyaKonno 0:158dbfb66e62 154 //Globle variable for Digital sensor process
TetsuyaKonno 0:158dbfb66e62 155 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 156 volatile int Sensor_X[8][6];
TetsuyaKonno 0:158dbfb66e62 157 volatile unsigned char sensor_value;
TetsuyaKonno 0:158dbfb66e62 158
TetsuyaKonno 0:158dbfb66e62 159 //Globle variable for Mark detection process
TetsuyaKonno 0:158dbfb66e62 160 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 161 double TempDevi_Triangle[15];
TetsuyaKonno 0:158dbfb66e62 162 unsigned char TempBinary_Triangle[15] = {0,1,1,1,0,
TetsuyaKonno 0:158dbfb66e62 163 0,0,1,0,0,
TetsuyaKonno 0:158dbfb66e62 164 0,0,0,0,0};
TetsuyaKonno 0:158dbfb66e62 165
TetsuyaKonno 0:158dbfb66e62 166 double NowDevi[15];
TetsuyaKonno 0:158dbfb66e62 167 unsigned char NowImageBinary[15];
TetsuyaKonno 0:158dbfb66e62 168
TetsuyaKonno 0:158dbfb66e62 169 volatile double retDevi_Triangle;
TetsuyaKonno 0:158dbfb66e62 170
TetsuyaKonno 0:158dbfb66e62 171 volatile double retDevi;
TetsuyaKonno 0:158dbfb66e62 172 volatile double retCovari;
TetsuyaKonno 0:158dbfb66e62 173 volatile int retJudgeIM;
TetsuyaKonno 0:158dbfb66e62 174 volatile int retJudgeIM_Max[1];
TetsuyaKonno 0:158dbfb66e62 175
TetsuyaKonno 0:158dbfb66e62 176 int Xt, Yt;
TetsuyaKonno 0:158dbfb66e62 177
TetsuyaKonno 0:158dbfb66e62 178 //Globle variable for led fanction
TetsuyaKonno 0:158dbfb66e62 179 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 180 volatile int led_set; /* Status */
TetsuyaKonno 0:158dbfb66e62 181
TetsuyaKonno 0:158dbfb66e62 182 // LED, OnTime, OffTime,
TetsuyaKonno 0:158dbfb66e62 183 volatile int led_data[10][3]= {LED_RED, 50, 50, /* ERROR */
TetsuyaKonno 0:158dbfb66e62 184 LED_RED, 500, 0, /* STOP */
TetsuyaKonno 0:158dbfb66e62 185 LED_GREEN, 500, 500, /* RUN */
TetsuyaKonno 0:158dbfb66e62 186 LED_BLUE, 50, 50, /* DEBUG */
TetsuyaKonno 0:158dbfb66e62 187 LED_GREEN, 1, 0, /* MOTOR_START */
TetsuyaKonno 0:158dbfb66e62 188 LED_RED, 1, 0, /* MOTOR_STOP */
TetsuyaKonno 0:158dbfb66e62 189 LED_WHITE, 500, 500}; /* MARK_T */
TetsuyaKonno 0:158dbfb66e62 190
TetsuyaKonno 0:158dbfb66e62 191 //Globle variable for Trace program
TetsuyaKonno 0:158dbfb66e62 192 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 193 volatile unsigned long cnt0; /* Used by timer function */
TetsuyaKonno 0:158dbfb66e62 194 volatile unsigned long cnt1; /* Used within main */
TetsuyaKonno 0:158dbfb66e62 195 volatile int pattern; /* Pattern numbers */
TetsuyaKonno 0:158dbfb66e62 196 volatile int handle_buff;
TetsuyaKonno 0:158dbfb66e62 197
TetsuyaKonno 0:158dbfb66e62 198 const int revolution_difference[] = {
TetsuyaKonno 0:158dbfb66e62 199 100, 98, 97, 95, 93,
TetsuyaKonno 0:158dbfb66e62 200 92, 90, 88, 87, 85,
TetsuyaKonno 0:158dbfb66e62 201 84, 82, 81, 79, 78,
TetsuyaKonno 0:158dbfb66e62 202 76, 75, 73, 72, 71,
TetsuyaKonno 0:158dbfb66e62 203 69, 68, 66, 65, 64,
TetsuyaKonno 0:158dbfb66e62 204 62, 61, 59, 58, 57,
TetsuyaKonno 0:158dbfb66e62 205 55, 54, 52, 51, 50,
TetsuyaKonno 0:158dbfb66e62 206 48, 47, 45, 44, 42,
TetsuyaKonno 0:158dbfb66e62 207 41, 39, 38, 36, 35,
TetsuyaKonno 0:158dbfb66e62 208 33 };
TetsuyaKonno 0:158dbfb66e62 209
TetsuyaKonno 0:158dbfb66e62 210 //******************************************************************//
TetsuyaKonno 0:158dbfb66e62 211 // Main function
TetsuyaKonno 0:158dbfb66e62 212 //*******************************************************************/
TetsuyaKonno 0:158dbfb66e62 213 int main( void )
TetsuyaKonno 0:158dbfb66e62 214 {
TetsuyaKonno 0:158dbfb66e62 215 /* NTSC-Video */
TetsuyaKonno 0:158dbfb66e62 216 DisplayBase::graphics_error_t error;
TetsuyaKonno 0:158dbfb66e62 217
TetsuyaKonno 0:158dbfb66e62 218 /* Create DisplayBase object */
TetsuyaKonno 0:158dbfb66e62 219 DisplayBase Display;
TetsuyaKonno 0:158dbfb66e62 220
TetsuyaKonno 0:158dbfb66e62 221 /* Graphics initialization process */
TetsuyaKonno 0:158dbfb66e62 222 error = Display.Graphics_init(NULL);
TetsuyaKonno 0:158dbfb66e62 223 if (error != DisplayBase::GRAPHICS_OK) {
TetsuyaKonno 0:158dbfb66e62 224 printf("Line %d, error %d\n", __LINE__, error);
TetsuyaKonno 0:158dbfb66e62 225 while (1);
TetsuyaKonno 0:158dbfb66e62 226 }
TetsuyaKonno 0:158dbfb66e62 227
TetsuyaKonno 0:158dbfb66e62 228 error = Display.Graphics_Video_init( DisplayBase::INPUT_SEL_VDEC, NULL);
TetsuyaKonno 0:158dbfb66e62 229 if( error != DisplayBase::GRAPHICS_OK ) {
TetsuyaKonno 0:158dbfb66e62 230 while(1);
TetsuyaKonno 0:158dbfb66e62 231 }
TetsuyaKonno 0:158dbfb66e62 232
TetsuyaKonno 0:158dbfb66e62 233 /* Interrupt callback function setting (Vsync signal input to scaler 0) */
TetsuyaKonno 0:158dbfb66e62 234 error = Display.Graphics_Irq_Handler_Set(DisplayBase::INT_TYPE_S0_VI_VSYNC, 0, IntCallbackFunc_Vsync);
TetsuyaKonno 0:158dbfb66e62 235 if (error != DisplayBase::GRAPHICS_OK) {
TetsuyaKonno 0:158dbfb66e62 236 printf("Line %d, error %d\n", __LINE__, error);
TetsuyaKonno 0:158dbfb66e62 237 while (1);
TetsuyaKonno 0:158dbfb66e62 238 }
TetsuyaKonno 0:158dbfb66e62 239
TetsuyaKonno 0:158dbfb66e62 240 /* Video capture setting (progressive form fixed) */
TetsuyaKonno 0:158dbfb66e62 241 error = Display.Video_Write_Setting(
TetsuyaKonno 0:158dbfb66e62 242 VIDEO_INPUT_CH,
TetsuyaKonno 0:158dbfb66e62 243 DisplayBase::COL_SYS_NTSC_358,
TetsuyaKonno 0:158dbfb66e62 244 write_buff_addr,
TetsuyaKonno 0:158dbfb66e62 245 VIDEO_BUFFER_STRIDE,
TetsuyaKonno 0:158dbfb66e62 246 DisplayBase::VIDEO_FORMAT_YCBCR422,
TetsuyaKonno 0:158dbfb66e62 247 DisplayBase::WR_RD_WRSWA_32_16BIT,
TetsuyaKonno 0:158dbfb66e62 248 PIXEL_VW,
TetsuyaKonno 0:158dbfb66e62 249 PIXEL_HW
TetsuyaKonno 0:158dbfb66e62 250 );
TetsuyaKonno 0:158dbfb66e62 251 if (error != DisplayBase::GRAPHICS_OK) {
TetsuyaKonno 0:158dbfb66e62 252 printf("Line %d, error %d\n", __LINE__, error);
TetsuyaKonno 0:158dbfb66e62 253 while (1);
TetsuyaKonno 0:158dbfb66e62 254 }
TetsuyaKonno 0:158dbfb66e62 255
TetsuyaKonno 0:158dbfb66e62 256 /* Interrupt callback function setting (Field end signal for recording function in scaler 0) */
TetsuyaKonno 0:158dbfb66e62 257 error = Display.Graphics_Irq_Handler_Set(VIDEO_INT_TYPE, 0, IntCallbackFunc_Vfield);
TetsuyaKonno 0:158dbfb66e62 258 if (error != DisplayBase::GRAPHICS_OK) {
TetsuyaKonno 0:158dbfb66e62 259 printf("Line %d, error %d\n", __LINE__, error);
TetsuyaKonno 0:158dbfb66e62 260 while (1);
TetsuyaKonno 0:158dbfb66e62 261 }
TetsuyaKonno 0:158dbfb66e62 262
TetsuyaKonno 0:158dbfb66e62 263 /* Video write process start */
TetsuyaKonno 0:158dbfb66e62 264 error = Display.Video_Start (VIDEO_INPUT_CH);
TetsuyaKonno 0:158dbfb66e62 265 if (error != DisplayBase::GRAPHICS_OK) {
TetsuyaKonno 0:158dbfb66e62 266 printf("Line %d, error %d\n", __LINE__, error);
TetsuyaKonno 0:158dbfb66e62 267 while (1);
TetsuyaKonno 0:158dbfb66e62 268 }
TetsuyaKonno 0:158dbfb66e62 269
TetsuyaKonno 0:158dbfb66e62 270 /* Video write process stop */
TetsuyaKonno 0:158dbfb66e62 271 error = Display.Video_Stop (VIDEO_INPUT_CH);
TetsuyaKonno 0:158dbfb66e62 272 if (error != DisplayBase::GRAPHICS_OK) {
TetsuyaKonno 0:158dbfb66e62 273 printf("Line %d, error %d\n", __LINE__, error);
TetsuyaKonno 0:158dbfb66e62 274 while (1);
TetsuyaKonno 0:158dbfb66e62 275 }
TetsuyaKonno 0:158dbfb66e62 276
TetsuyaKonno 0:158dbfb66e62 277 /* Video write process start */
TetsuyaKonno 0:158dbfb66e62 278 error = Display.Video_Start (VIDEO_INPUT_CH);
TetsuyaKonno 0:158dbfb66e62 279 if (error != DisplayBase::GRAPHICS_OK) {
TetsuyaKonno 0:158dbfb66e62 280 printf("Line %d, error %d\n", __LINE__, error);
TetsuyaKonno 0:158dbfb66e62 281 while (1);
TetsuyaKonno 0:158dbfb66e62 282 }
TetsuyaKonno 0:158dbfb66e62 283
TetsuyaKonno 0:158dbfb66e62 284 /* Wait vsync to update resister */
TetsuyaKonno 0:158dbfb66e62 285 WaitVsync(1);
TetsuyaKonno 0:158dbfb66e62 286
TetsuyaKonno 0:158dbfb66e62 287 /* Wait 2 Vfield(Top or bottom field) */
TetsuyaKonno 0:158dbfb66e62 288 WaitVfield(2);
TetsuyaKonno 0:158dbfb66e62 289
TetsuyaKonno 0:158dbfb66e62 290 /* Initialize MCU functions */
TetsuyaKonno 0:158dbfb66e62 291 init_MTU2_PWM_Motor();
TetsuyaKonno 0:158dbfb66e62 292 init_MTU2_PWM_Servo();
TetsuyaKonno 0:158dbfb66e62 293 interrput.attach(&intTimer, 0.001);
TetsuyaKonno 0:158dbfb66e62 294 pc.baud(230400);
TetsuyaKonno 0:158dbfb66e62 295
TetsuyaKonno 0:158dbfb66e62 296 /* Initialize Micon Car state */
TetsuyaKonno 0:158dbfb66e62 297 led_out( 0x0 );
TetsuyaKonno 0:158dbfb66e62 298 handle( 0 );
TetsuyaKonno 0:158dbfb66e62 299 motor( 0, 0 );
TetsuyaKonno 0:158dbfb66e62 300
TetsuyaKonno 0:158dbfb66e62 301 /* wait to stabilize NTSC signal (about 170ms) */
TetsuyaKonno 0:158dbfb66e62 302 wait(0.2);
TetsuyaKonno 0:158dbfb66e62 303
TetsuyaKonno 0:158dbfb66e62 304 /* Initialize Digital sensor */
TetsuyaKonno 0:158dbfb66e62 305 CenterLine_Corrective( ImageBinary );
TetsuyaKonno 0:158dbfb66e62 306
TetsuyaKonno 0:158dbfb66e62 307 /* Initialize Mark detection */
TetsuyaKonno 0:158dbfb66e62 308 retDevi_Triangle = Standard_Deviation( TempBinary_Triangle, TempDevi_Triangle, 15 );
TetsuyaKonno 0:158dbfb66e62 309
TetsuyaKonno 0:158dbfb66e62 310 if( user_button_get() ) {
TetsuyaKonno 0:158dbfb66e62 311 wait(0.1);
TetsuyaKonno 0:158dbfb66e62 312 led_m_set( DEBUG );
TetsuyaKonno 0:158dbfb66e62 313 while( user_button_get() );
TetsuyaKonno 0:158dbfb66e62 314 wait(0.5);
TetsuyaKonno 0:158dbfb66e62 315 pc.printf( "Please push the SW ( on the Motor drive board )\n\r" );
TetsuyaKonno 0:158dbfb66e62 316 pc.printf( "\n\r" );
TetsuyaKonno 0:158dbfb66e62 317 while( !user_button_get() );
TetsuyaKonno 0:158dbfb66e62 318 while( 1 ){
TetsuyaKonno 0:158dbfb66e62 319 ImageData_Serial_Out2( ImageBinary, 20 );
TetsuyaKonno 0:158dbfb66e62 320 }
TetsuyaKonno 0:158dbfb66e62 321 }
TetsuyaKonno 0:158dbfb66e62 322
TetsuyaKonno 0:158dbfb66e62 323 led_m_set( RUN );
TetsuyaKonno 0:158dbfb66e62 324
TetsuyaKonno 0:158dbfb66e62 325 while( 1 ) {
TetsuyaKonno 0:158dbfb66e62 326
TetsuyaKonno 0:158dbfb66e62 327 switch( pattern ) {
TetsuyaKonno 0:158dbfb66e62 328 /*****************************************************************
TetsuyaKonno 0:158dbfb66e62 329 About patern
TetsuyaKonno 0:158dbfb66e62 330 0:wait for switch input
TetsuyaKonno 0:158dbfb66e62 331 1:check if start bar is open
TetsuyaKonno 0:158dbfb66e62 332 11:normal trace
TetsuyaKonno 0:158dbfb66e62 333 12:Left side
TetsuyaKonno 0:158dbfb66e62 334 13:right side
TetsuyaKonno 0:158dbfb66e62 335 *****************************************************************/
TetsuyaKonno 0:158dbfb66e62 336 case 0:
TetsuyaKonno 0:158dbfb66e62 337 /* wait for switch input */
TetsuyaKonno 0:158dbfb66e62 338 if( pushsw_get() ) {
TetsuyaKonno 0:158dbfb66e62 339 led_out( 0x0 );
TetsuyaKonno 0:158dbfb66e62 340 led_m_set( RUN );
TetsuyaKonno 0:158dbfb66e62 341 pattern = 11;
TetsuyaKonno 0:158dbfb66e62 342 cnt1 = 0;
TetsuyaKonno 0:158dbfb66e62 343 break;
TetsuyaKonno 0:158dbfb66e62 344 }
TetsuyaKonno 0:158dbfb66e62 345 if( cnt1 < 100 ) {
TetsuyaKonno 0:158dbfb66e62 346 led_out( 0x1 );
TetsuyaKonno 0:158dbfb66e62 347 } else if( cnt1 < 200 ) {
TetsuyaKonno 0:158dbfb66e62 348 led_out( 0x2 );
TetsuyaKonno 0:158dbfb66e62 349 } else {
TetsuyaKonno 0:158dbfb66e62 350 cnt1 = 0;
TetsuyaKonno 0:158dbfb66e62 351 }
TetsuyaKonno 0:158dbfb66e62 352 break;
TetsuyaKonno 0:158dbfb66e62 353
TetsuyaKonno 0:158dbfb66e62 354 case 11:
TetsuyaKonno 0:158dbfb66e62 355 /* normal trace */
TetsuyaKonno 0:158dbfb66e62 356 if( MarkCheck_Triangle( 90 ) ) {
TetsuyaKonno 0:158dbfb66e62 357 led_m_set( MARK_T );
TetsuyaKonno 0:158dbfb66e62 358 pattern = 91;
TetsuyaKonno 0:158dbfb66e62 359 break;
TetsuyaKonno 0:158dbfb66e62 360 }
TetsuyaKonno 0:158dbfb66e62 361 switch( (digital_sensor()&0x0f) ) {
TetsuyaKonno 0:158dbfb66e62 362 case 0x00:
TetsuyaKonno 0:158dbfb66e62 363 handle( 0 );
TetsuyaKonno 0:158dbfb66e62 364 motor( 100, 100 );
TetsuyaKonno 0:158dbfb66e62 365 break;
TetsuyaKonno 0:158dbfb66e62 366 case 0x02:
TetsuyaKonno 0:158dbfb66e62 367 handle( 3 );
TetsuyaKonno 0:158dbfb66e62 368 motor( 100, diff(100) );
TetsuyaKonno 0:158dbfb66e62 369 break;
TetsuyaKonno 0:158dbfb66e62 370 case 0x03:
TetsuyaKonno 0:158dbfb66e62 371 handle( 12 );
TetsuyaKonno 0:158dbfb66e62 372 motor( 100, diff(100) );
TetsuyaKonno 0:158dbfb66e62 373 break;
TetsuyaKonno 0:158dbfb66e62 374 case 0x01:
TetsuyaKonno 0:158dbfb66e62 375 handle( 20 );
TetsuyaKonno 0:158dbfb66e62 376 motor( 100, diff(100) );
TetsuyaKonno 0:158dbfb66e62 377 pattern = 12;
TetsuyaKonno 0:158dbfb66e62 378 break;
TetsuyaKonno 0:158dbfb66e62 379 case 0x04:
TetsuyaKonno 0:158dbfb66e62 380 handle( -3 );
TetsuyaKonno 0:158dbfb66e62 381 motor( diff(100), 100 );
TetsuyaKonno 0:158dbfb66e62 382 break;
TetsuyaKonno 0:158dbfb66e62 383 case 0x0c:
TetsuyaKonno 0:158dbfb66e62 384 handle( -12 );
TetsuyaKonno 0:158dbfb66e62 385 motor( diff(100), 100 );
TetsuyaKonno 0:158dbfb66e62 386 break;
TetsuyaKonno 0:158dbfb66e62 387 case 0x08:
TetsuyaKonno 0:158dbfb66e62 388 handle( -20 );
TetsuyaKonno 0:158dbfb66e62 389 motor( diff(100), 100 );
TetsuyaKonno 0:158dbfb66e62 390 pattern = 13;
TetsuyaKonno 0:158dbfb66e62 391 break;
TetsuyaKonno 0:158dbfb66e62 392 default:
TetsuyaKonno 0:158dbfb66e62 393 break;
TetsuyaKonno 0:158dbfb66e62 394 }
TetsuyaKonno 0:158dbfb66e62 395 break;
TetsuyaKonno 0:158dbfb66e62 396
TetsuyaKonno 0:158dbfb66e62 397 case 12:
TetsuyaKonno 0:158dbfb66e62 398 /* Left side */
TetsuyaKonno 0:158dbfb66e62 399 if( (digital_sensor()&0x02) == 0x02 ) {
TetsuyaKonno 0:158dbfb66e62 400 pattern = 11;
TetsuyaKonno 0:158dbfb66e62 401 break;
TetsuyaKonno 0:158dbfb66e62 402 }
TetsuyaKonno 0:158dbfb66e62 403 switch( (digital_sensor()&0x0f) ) {
TetsuyaKonno 0:158dbfb66e62 404 case 0x01:
TetsuyaKonno 0:158dbfb66e62 405 handle( 20 );
TetsuyaKonno 0:158dbfb66e62 406 motor( 100, diff(100) );
TetsuyaKonno 0:158dbfb66e62 407 break;
TetsuyaKonno 0:158dbfb66e62 408 case 0x00:
TetsuyaKonno 0:158dbfb66e62 409 case 0x08:
TetsuyaKonno 0:158dbfb66e62 410 case 0x0c:
TetsuyaKonno 0:158dbfb66e62 411 handle( 22 );
TetsuyaKonno 0:158dbfb66e62 412 motor( 100, diff(100) );
TetsuyaKonno 0:158dbfb66e62 413 break;
TetsuyaKonno 0:158dbfb66e62 414 default:
TetsuyaKonno 0:158dbfb66e62 415 break;
TetsuyaKonno 0:158dbfb66e62 416 }
TetsuyaKonno 0:158dbfb66e62 417 break;
TetsuyaKonno 0:158dbfb66e62 418
TetsuyaKonno 0:158dbfb66e62 419 case 13:
TetsuyaKonno 0:158dbfb66e62 420 /* right side */
TetsuyaKonno 0:158dbfb66e62 421 if( (digital_sensor()&0x04) == 0x04 ) {
TetsuyaKonno 0:158dbfb66e62 422 pattern = 11;
TetsuyaKonno 0:158dbfb66e62 423 }
TetsuyaKonno 0:158dbfb66e62 424 switch( (digital_sensor()&0x0f) ) {
TetsuyaKonno 0:158dbfb66e62 425 case 0x08:
TetsuyaKonno 0:158dbfb66e62 426 handle( -20 );
TetsuyaKonno 0:158dbfb66e62 427 motor( diff(100), 100 );
TetsuyaKonno 0:158dbfb66e62 428 break;
TetsuyaKonno 0:158dbfb66e62 429 case 0x00:
TetsuyaKonno 0:158dbfb66e62 430 case 0x01:
TetsuyaKonno 0:158dbfb66e62 431 case 0x03:
TetsuyaKonno 0:158dbfb66e62 432 handle( -22 );
TetsuyaKonno 0:158dbfb66e62 433 motor( diff(100), 100 );
TetsuyaKonno 0:158dbfb66e62 434 break;
TetsuyaKonno 0:158dbfb66e62 435 default:
TetsuyaKonno 0:158dbfb66e62 436 break;
TetsuyaKonno 0:158dbfb66e62 437 }
TetsuyaKonno 0:158dbfb66e62 438 break;
TetsuyaKonno 0:158dbfb66e62 439
TetsuyaKonno 0:158dbfb66e62 440 case 91:
TetsuyaKonno 0:158dbfb66e62 441 /* Stop */
TetsuyaKonno 0:158dbfb66e62 442 handle( 0 );
TetsuyaKonno 0:158dbfb66e62 443 motor( 0, 0 );
TetsuyaKonno 0:158dbfb66e62 444 break;
TetsuyaKonno 0:158dbfb66e62 445
TetsuyaKonno 0:158dbfb66e62 446 default:
TetsuyaKonno 0:158dbfb66e62 447 break;
TetsuyaKonno 0:158dbfb66e62 448 }
TetsuyaKonno 0:158dbfb66e62 449 }
TetsuyaKonno 0:158dbfb66e62 450 }
TetsuyaKonno 0:158dbfb66e62 451
TetsuyaKonno 0:158dbfb66e62 452 //******************************************************************//
TetsuyaKonno 0:158dbfb66e62 453 // Initialize functions
TetsuyaKonno 0:158dbfb66e62 454 //*******************************************************************/
TetsuyaKonno 0:158dbfb66e62 455 //Initialize MTU2 PWM functions
TetsuyaKonno 0:158dbfb66e62 456 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 457 //MTU2_3, MTU2_4
TetsuyaKonno 0:158dbfb66e62 458 //Reset-Synchronized PWM mode
TetsuyaKonno 0:158dbfb66e62 459 //TIOC4A(P4_4) :Left-motor
TetsuyaKonno 0:158dbfb66e62 460 //TIOC4B(P4_5) :Right-motor
TetsuyaKonno 0:158dbfb66e62 461 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 462 void init_MTU2_PWM_Motor( void )
TetsuyaKonno 0:158dbfb66e62 463 {
TetsuyaKonno 0:158dbfb66e62 464 /* Port setting for S/W I/O Contorol */
TetsuyaKonno 0:158dbfb66e62 465 /* alternative mode */
TetsuyaKonno 0:158dbfb66e62 466
TetsuyaKonno 0:158dbfb66e62 467 /* MTU2_4 (P4_4)(P4_5) */
TetsuyaKonno 0:158dbfb66e62 468 GPIOPBDC4 = 0x0000; /* Bidirection mode disabled*/
TetsuyaKonno 0:158dbfb66e62 469 GPIOPFCAE4 &= 0xffcf; /* The alternative function of a pin */
TetsuyaKonno 0:158dbfb66e62 470 GPIOPFCE4 |= 0x0030; /* The alternative function of a pin */
TetsuyaKonno 0:158dbfb66e62 471 GPIOPFC4 &= 0xffcf; /* The alternative function of a pin */
TetsuyaKonno 0:158dbfb66e62 472 /* 2nd altemative function/output */
TetsuyaKonno 0:158dbfb66e62 473 GPIOP4 &= 0xffcf; /* */
TetsuyaKonno 0:158dbfb66e62 474 GPIOPM4 &= 0xffcf; /* p4_4,P4_5:output */
TetsuyaKonno 0:158dbfb66e62 475 GPIOPMC4 |= 0x0030; /* P4_4,P4_5:double */
TetsuyaKonno 0:158dbfb66e62 476
TetsuyaKonno 0:158dbfb66e62 477 /* Mosule stop 33(MTU2) canceling */
TetsuyaKonno 0:158dbfb66e62 478 CPGSTBCR3 &= 0xf7;
TetsuyaKonno 0:158dbfb66e62 479
TetsuyaKonno 0:158dbfb66e62 480 /* MTU2_3 and MTU2_4 (Motor PWM) */
TetsuyaKonno 0:158dbfb66e62 481 MTU2TCR_3 = 0x20; /* TCNT Clear(TGRA), P0φ/1 */
TetsuyaKonno 0:158dbfb66e62 482 MTU2TOCR1 = 0x04; /* */
TetsuyaKonno 0:158dbfb66e62 483 MTU2TOCR2 = 0x40; /* N L>H P H>L */
TetsuyaKonno 0:158dbfb66e62 484 MTU2TMDR_3 = 0x38; /* Buff:ON Reset-Synchronized PWM mode */
TetsuyaKonno 0:158dbfb66e62 485 MTU2TMDR_4 = 0x30; /* Buff:ON */
TetsuyaKonno 0:158dbfb66e62 486 MTU2TOER = 0xc6; /* TIOC3B,4A,4B enabled output */
TetsuyaKonno 0:158dbfb66e62 487 MTU2TCNT_3 = MTU2TCNT_4 = 0; /* TCNT3,TCNT4 Set 0 */
TetsuyaKonno 0:158dbfb66e62 488 MTU2TGRA_3 = MTU2TGRC_3 = MOTOR_PWM_CYCLE;
TetsuyaKonno 0:158dbfb66e62 489 /* PWM-Cycle(1ms) */
TetsuyaKonno 0:158dbfb66e62 490 MTU2TGRA_4 = MTU2TGRC_4 = 0; /* Left-motor(P4_4) */
TetsuyaKonno 0:158dbfb66e62 491 MTU2TGRB_4 = MTU2TGRD_4 = 0; /* Right-motor(P4_5) */
TetsuyaKonno 0:158dbfb66e62 492 MTU2TSTR |= 0x40; /* TCNT_4 Start */
TetsuyaKonno 0:158dbfb66e62 493 }
TetsuyaKonno 0:158dbfb66e62 494
TetsuyaKonno 0:158dbfb66e62 495 //Initialize MTU2 PWM functions
TetsuyaKonno 0:158dbfb66e62 496 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 497 //MTU2_0
TetsuyaKonno 0:158dbfb66e62 498 //PWM mode 1
TetsuyaKonno 0:158dbfb66e62 499 //TIOC0A(P4_0) :Servo-motor
TetsuyaKonno 0:158dbfb66e62 500 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 501 void init_MTU2_PWM_Servo( void )
TetsuyaKonno 0:158dbfb66e62 502 {
TetsuyaKonno 0:158dbfb66e62 503 /* Port setting for S/W I/O Contorol */
TetsuyaKonno 0:158dbfb66e62 504 /* alternative mode */
TetsuyaKonno 0:158dbfb66e62 505
TetsuyaKonno 0:158dbfb66e62 506 /* MTU2_0 (P4_0) */
TetsuyaKonno 0:158dbfb66e62 507 GPIOPBDC4 = 0x0000; /* Bidirection mode disabled*/
TetsuyaKonno 0:158dbfb66e62 508 GPIOPFCAE4 &= 0xfffe; /* The alternative function of a pin */
TetsuyaKonno 0:158dbfb66e62 509 GPIOPFCE4 &= 0xfffe; /* The alternative function of a pin */
TetsuyaKonno 0:158dbfb66e62 510 GPIOPFC4 |= 0x0001; /* The alternative function of a pin */
TetsuyaKonno 0:158dbfb66e62 511 /* 2nd alternative function/output */
TetsuyaKonno 0:158dbfb66e62 512 GPIOP4 &= 0xfffe; /* */
TetsuyaKonno 0:158dbfb66e62 513 GPIOPM4 &= 0xfffe; /* p4_0:output */
TetsuyaKonno 0:158dbfb66e62 514 GPIOPMC4 |= 0x0001; /* P4_0:double */
TetsuyaKonno 0:158dbfb66e62 515
TetsuyaKonno 0:158dbfb66e62 516 /* Mosule stop 33(MTU2) canceling */
TetsuyaKonno 0:158dbfb66e62 517 CPGSTBCR3 &= 0xf7;
TetsuyaKonno 0:158dbfb66e62 518
TetsuyaKonno 0:158dbfb66e62 519 /* MTU2_0 (Motor PWM) */
TetsuyaKonno 0:158dbfb66e62 520 MTU2TCR_0 = 0x22; /* TCNT Clear(TGRA), P0φ/16 */
TetsuyaKonno 0:158dbfb66e62 521 MTU2TIORH_0 = 0x52; /* TGRA L>H, TGRB H>L */
TetsuyaKonno 0:158dbfb66e62 522 MTU2TMDR_0 = 0x32; /* TGRC and TGRD = Buff-mode*/
TetsuyaKonno 0:158dbfb66e62 523 /* PWM-mode1 */
TetsuyaKonno 0:158dbfb66e62 524 MTU2TCNT_0 = 0; /* TCNT0 Set 0 */
TetsuyaKonno 0:158dbfb66e62 525 MTU2TGRA_0 = MTU2TGRC_0 = SERVO_PWM_CYCLE;
TetsuyaKonno 0:158dbfb66e62 526 /* PWM-Cycle(16ms) */
TetsuyaKonno 0:158dbfb66e62 527 MTU2TGRB_0 = MTU2TGRD_0 = 0; /* Servo-motor(P4_0) */
TetsuyaKonno 0:158dbfb66e62 528 MTU2TSTR |= 0x01; /* TCNT_0 Start */
TetsuyaKonno 0:158dbfb66e62 529 }
TetsuyaKonno 0:158dbfb66e62 530
TetsuyaKonno 0:158dbfb66e62 531 //******************************************************************//
TetsuyaKonno 0:158dbfb66e62 532 // @brief Interrupt callback function
TetsuyaKonno 0:158dbfb66e62 533 // @param[in] int_type : VDC5 interrupt type
TetsuyaKonno 0:158dbfb66e62 534 // @retval None
TetsuyaKonno 0:158dbfb66e62 535 //*******************************************************************/
TetsuyaKonno 0:158dbfb66e62 536 static void IntCallbackFunc_Vfield(DisplayBase::int_type_t int_type)
TetsuyaKonno 0:158dbfb66e62 537 {
TetsuyaKonno 0:158dbfb66e62 538 if (vfield_count > 0) {
TetsuyaKonno 0:158dbfb66e62 539 vfield_count--;
TetsuyaKonno 0:158dbfb66e62 540 }
TetsuyaKonno 0:158dbfb66e62 541 /* top or bottom (Change) */
TetsuyaKonno 0:158dbfb66e62 542 if ( vfield_count2 == 0 ) vfield_count2 = 1;
TetsuyaKonno 0:158dbfb66e62 543 else if ( vfield_count2 == 1 ) vfield_count2 = 0;
TetsuyaKonno 0:158dbfb66e62 544 }
TetsuyaKonno 0:158dbfb66e62 545
TetsuyaKonno 0:158dbfb66e62 546 //******************************************************************//
TetsuyaKonno 0:158dbfb66e62 547 // @brief Wait for the specified number of times Vsync occurs
TetsuyaKonno 0:158dbfb66e62 548 // @param[in] wait_count : Wait count
TetsuyaKonno 0:158dbfb66e62 549 // @retval None
TetsuyaKonno 0:158dbfb66e62 550 //*******************************************************************/
TetsuyaKonno 0:158dbfb66e62 551 static void WaitVfield(const int32_t wait_count)
TetsuyaKonno 0:158dbfb66e62 552 {
TetsuyaKonno 0:158dbfb66e62 553 vfield_count = wait_count;
TetsuyaKonno 0:158dbfb66e62 554 while (vfield_count > 0) {
TetsuyaKonno 0:158dbfb66e62 555 /* Do nothing */
TetsuyaKonno 0:158dbfb66e62 556 }
TetsuyaKonno 0:158dbfb66e62 557 }
TetsuyaKonno 0:158dbfb66e62 558
TetsuyaKonno 0:158dbfb66e62 559 //******************************************************************//
TetsuyaKonno 0:158dbfb66e62 560 // @brief Interrupt callback function for Vsync interruption
TetsuyaKonno 0:158dbfb66e62 561 // @param[in] int_type : VDC5 interrupt type
TetsuyaKonno 0:158dbfb66e62 562 // @retval None
TetsuyaKonno 0:158dbfb66e62 563 //*******************************************************************/
TetsuyaKonno 0:158dbfb66e62 564 static void IntCallbackFunc_Vsync(DisplayBase::int_type_t int_type)
TetsuyaKonno 0:158dbfb66e62 565 {
TetsuyaKonno 0:158dbfb66e62 566 if (vsync_count > 0) {
TetsuyaKonno 0:158dbfb66e62 567 vsync_count--;
TetsuyaKonno 0:158dbfb66e62 568 }
TetsuyaKonno 0:158dbfb66e62 569 }
TetsuyaKonno 0:158dbfb66e62 570
TetsuyaKonno 0:158dbfb66e62 571 //******************************************************************//
TetsuyaKonno 0:158dbfb66e62 572 // @brief Wait for the specified number of times Vsync occurs
TetsuyaKonno 0:158dbfb66e62 573 // @param[in] wait_count : Wait count
TetsuyaKonno 0:158dbfb66e62 574 // @retval None
TetsuyaKonno 0:158dbfb66e62 575 //*******************************************************************/
TetsuyaKonno 0:158dbfb66e62 576 static void WaitVsync(const int32_t wait_count)
TetsuyaKonno 0:158dbfb66e62 577 {
TetsuyaKonno 0:158dbfb66e62 578 vsync_count = wait_count;
TetsuyaKonno 0:158dbfb66e62 579 while (vsync_count > 0) {
TetsuyaKonno 0:158dbfb66e62 580 /* Do nothing */
TetsuyaKonno 0:158dbfb66e62 581 }
TetsuyaKonno 0:158dbfb66e62 582 }
TetsuyaKonno 0:158dbfb66e62 583
TetsuyaKonno 0:158dbfb66e62 584 //******************************************************************//
TetsuyaKonno 0:158dbfb66e62 585 // Interrupt function( intTimer )
TetsuyaKonno 0:158dbfb66e62 586 //*******************************************************************/
TetsuyaKonno 0:158dbfb66e62 587 void intTimer( void )
TetsuyaKonno 0:158dbfb66e62 588 {
TetsuyaKonno 0:158dbfb66e62 589 static int counter = 0;
TetsuyaKonno 0:158dbfb66e62 590
TetsuyaKonno 0:158dbfb66e62 591 cnt0++;
TetsuyaKonno 0:158dbfb66e62 592 cnt1++;
TetsuyaKonno 0:158dbfb66e62 593
TetsuyaKonno 0:158dbfb66e62 594 /* field check */
TetsuyaKonno 0:158dbfb66e62 595 if( vfield_count2 == vfield_count2_buff ) {
TetsuyaKonno 0:158dbfb66e62 596 vfield_count2_buff = vfield_count2;
TetsuyaKonno 0:158dbfb66e62 597 }
TetsuyaKonno 0:158dbfb66e62 598 /* Top field */
TetsuyaKonno 0:158dbfb66e62 599 if( !vfield_count2 ) {
TetsuyaKonno 0:158dbfb66e62 600 led_m_user( 1 );
TetsuyaKonno 0:158dbfb66e62 601 switch( counter++ ) {
TetsuyaKonno 0:158dbfb66e62 602 case 0:
TetsuyaKonno 0:158dbfb66e62 603 Image_Extraction( write_buff_addr, ImageData_A, vfield_count2 );
TetsuyaKonno 0:158dbfb66e62 604 break;
TetsuyaKonno 0:158dbfb66e62 605 case 1:
TetsuyaKonno 0:158dbfb66e62 606 Image_Extraction( write_buff_addr, ImageData_A, vfield_count2 );
TetsuyaKonno 0:158dbfb66e62 607 break;
TetsuyaKonno 0:158dbfb66e62 608 case 2:
TetsuyaKonno 0:158dbfb66e62 609 Image_Reduction( ImageData_A, 160, ImageComp_A, 8 );
TetsuyaKonno 0:158dbfb66e62 610 break;
TetsuyaKonno 0:158dbfb66e62 611 case 3:
TetsuyaKonno 0:158dbfb66e62 612 Image_Reduction( ImageData_A, 160, ImageComp_A, 8 );
TetsuyaKonno 0:158dbfb66e62 613 break;
TetsuyaKonno 0:158dbfb66e62 614 case 4:
TetsuyaKonno 0:158dbfb66e62 615 Binarization_process( ImageComp_A, ImageBinary, 20*15, 128 );
TetsuyaKonno 0:158dbfb66e62 616 break;
TetsuyaKonno 0:158dbfb66e62 617 case 5:
TetsuyaKonno 0:158dbfb66e62 618 /* Trace by image processing */
TetsuyaKonno 0:158dbfb66e62 619 digital_sensor_process( ImageBinary );
TetsuyaKonno 0:158dbfb66e62 620 break;
TetsuyaKonno 0:158dbfb66e62 621 case 6:
TetsuyaKonno 0:158dbfb66e62 622 //MarkCheck_Triangle
TetsuyaKonno 0:158dbfb66e62 623 MarkDetect_process_T();
TetsuyaKonno 0:158dbfb66e62 624 break;
TetsuyaKonno 0:158dbfb66e62 625 case 15:
TetsuyaKonno 0:158dbfb66e62 626 counter = 0;
TetsuyaKonno 0:158dbfb66e62 627 break;
TetsuyaKonno 0:158dbfb66e62 628 default:
TetsuyaKonno 0:158dbfb66e62 629 break;
TetsuyaKonno 0:158dbfb66e62 630 }
TetsuyaKonno 0:158dbfb66e62 631 }
TetsuyaKonno 0:158dbfb66e62 632 /* bottom field */
TetsuyaKonno 0:158dbfb66e62 633 else {
TetsuyaKonno 0:158dbfb66e62 634 led_m_user( 0 );
TetsuyaKonno 0:158dbfb66e62 635 switch( counter++ ) {
TetsuyaKonno 0:158dbfb66e62 636 case 0:
TetsuyaKonno 0:158dbfb66e62 637 Image_Extraction( write_buff_addr, ImageData_A, vfield_count2 );
TetsuyaKonno 0:158dbfb66e62 638 break;
TetsuyaKonno 0:158dbfb66e62 639 case 1:
TetsuyaKonno 0:158dbfb66e62 640 Image_Extraction( write_buff_addr, ImageData_A, vfield_count2 );
TetsuyaKonno 0:158dbfb66e62 641 break;
TetsuyaKonno 0:158dbfb66e62 642 case 2:
TetsuyaKonno 0:158dbfb66e62 643 Image_Reduction( ImageData_A, 160, ImageComp_A, 8 );
TetsuyaKonno 0:158dbfb66e62 644 break;
TetsuyaKonno 0:158dbfb66e62 645 case 3:
TetsuyaKonno 0:158dbfb66e62 646 Image_Reduction( ImageData_A, 160, ImageComp_A, 8 );
TetsuyaKonno 0:158dbfb66e62 647 break;
TetsuyaKonno 0:158dbfb66e62 648 case 4:
TetsuyaKonno 0:158dbfb66e62 649 Binarization_process( ImageComp_A, ImageBinary, 20*15, 128 );
TetsuyaKonno 0:158dbfb66e62 650 break;
TetsuyaKonno 0:158dbfb66e62 651 case 5:
TetsuyaKonno 0:158dbfb66e62 652 /* Trace by image processing */
TetsuyaKonno 0:158dbfb66e62 653 digital_sensor_process( ImageBinary );
TetsuyaKonno 0:158dbfb66e62 654 break;
TetsuyaKonno 0:158dbfb66e62 655 case 6:
TetsuyaKonno 0:158dbfb66e62 656 //MarkCheck_Triangle
TetsuyaKonno 0:158dbfb66e62 657 MarkDetect_process_T();
TetsuyaKonno 0:158dbfb66e62 658 break;
TetsuyaKonno 0:158dbfb66e62 659 case 15:
TetsuyaKonno 0:158dbfb66e62 660 counter = 0;
TetsuyaKonno 0:158dbfb66e62 661 break;
TetsuyaKonno 0:158dbfb66e62 662 default:
TetsuyaKonno 0:158dbfb66e62 663 break;
TetsuyaKonno 0:158dbfb66e62 664 }
TetsuyaKonno 0:158dbfb66e62 665 }
TetsuyaKonno 0:158dbfb66e62 666
TetsuyaKonno 0:158dbfb66e62 667 /* LED processing */
TetsuyaKonno 0:158dbfb66e62 668 led_m_process();
TetsuyaKonno 0:158dbfb66e62 669 }
TetsuyaKonno 0:158dbfb66e62 670
TetsuyaKonno 0:158dbfb66e62 671 //******************************************************************//
TetsuyaKonno 0:158dbfb66e62 672 // functions ( on GR-PEACH board )
TetsuyaKonno 0:158dbfb66e62 673 //*******************************************************************/
TetsuyaKonno 0:158dbfb66e62 674 //led_rgb Function
TetsuyaKonno 0:158dbfb66e62 675 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 676 void led_rgb(int led)
TetsuyaKonno 0:158dbfb66e62 677 {
TetsuyaKonno 0:158dbfb66e62 678 LED_R = led & 0x1;
TetsuyaKonno 0:158dbfb66e62 679 LED_G = (led >> 1 ) & 0x1;
TetsuyaKonno 0:158dbfb66e62 680 LED_B = (led >> 2 ) & 0x1;
TetsuyaKonno 0:158dbfb66e62 681 }
TetsuyaKonno 0:158dbfb66e62 682
TetsuyaKonno 0:158dbfb66e62 683 //user_button_get Function
TetsuyaKonno 0:158dbfb66e62 684 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 685 unsigned int user_button_get( void )
TetsuyaKonno 0:158dbfb66e62 686 {
TetsuyaKonno 0:158dbfb66e62 687 return (~user_botton) & 0x1; /* Read ports with switches */
TetsuyaKonno 0:158dbfb66e62 688 }
TetsuyaKonno 0:158dbfb66e62 689
TetsuyaKonno 0:158dbfb66e62 690 //led_m_user Function
TetsuyaKonno 0:158dbfb66e62 691 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 692 void led_m_user( int led )
TetsuyaKonno 0:158dbfb66e62 693 {
TetsuyaKonno 0:158dbfb66e62 694 USER_LED = led & 0x01;
TetsuyaKonno 0:158dbfb66e62 695 }
TetsuyaKonno 0:158dbfb66e62 696
TetsuyaKonno 0:158dbfb66e62 697 //Lled_m_set Function
TetsuyaKonno 0:158dbfb66e62 698 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 699 void led_m_set( int set )
TetsuyaKonno 0:158dbfb66e62 700 {
TetsuyaKonno 0:158dbfb66e62 701 led_set = set;
TetsuyaKonno 0:158dbfb66e62 702 }
TetsuyaKonno 0:158dbfb66e62 703
TetsuyaKonno 0:158dbfb66e62 704 //led_m_process Function for only interrupt
TetsuyaKonno 0:158dbfb66e62 705 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 706 void led_m_process( void )
TetsuyaKonno 0:158dbfb66e62 707 {
TetsuyaKonno 0:158dbfb66e62 708 static unsigned long led_timer;
TetsuyaKonno 0:158dbfb66e62 709
TetsuyaKonno 0:158dbfb66e62 710 led_timer++;
TetsuyaKonno 0:158dbfb66e62 711
TetsuyaKonno 0:158dbfb66e62 712 /* Display */
TetsuyaKonno 0:158dbfb66e62 713 if( led_timer < led_data[led_set][1] ) led_rgb( led_data[led_set][0] );
TetsuyaKonno 0:158dbfb66e62 714 else if( led_timer < ( led_data[led_set][1] + led_data[led_set][2] ) ) led_rgb( LED_OFF );
TetsuyaKonno 0:158dbfb66e62 715 else led_timer = 0;
TetsuyaKonno 0:158dbfb66e62 716 }
TetsuyaKonno 0:158dbfb66e62 717
TetsuyaKonno 0:158dbfb66e62 718 //******************************************************************//
TetsuyaKonno 0:158dbfb66e62 719 // functions ( on Motor drive board )
TetsuyaKonno 0:158dbfb66e62 720 //*******************************************************************/
TetsuyaKonno 0:158dbfb66e62 721 //led_out Function
TetsuyaKonno 0:158dbfb66e62 722 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 723 void led_out(int led)
TetsuyaKonno 0:158dbfb66e62 724 {
TetsuyaKonno 0:158dbfb66e62 725 led = ~led;
TetsuyaKonno 0:158dbfb66e62 726 LED_3 = led & 0x1;
TetsuyaKonno 0:158dbfb66e62 727 LED_2 = ( led >> 1 ) & 0x1;
TetsuyaKonno 0:158dbfb66e62 728 }
TetsuyaKonno 0:158dbfb66e62 729
TetsuyaKonno 0:158dbfb66e62 730 //pushsw_get Function
TetsuyaKonno 0:158dbfb66e62 731 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 732 unsigned int pushsw_get( void )
TetsuyaKonno 0:158dbfb66e62 733 {
TetsuyaKonno 0:158dbfb66e62 734 return (~push_sw) & 0x1; /* Read ports with switches */
TetsuyaKonno 0:158dbfb66e62 735 }
TetsuyaKonno 0:158dbfb66e62 736
TetsuyaKonno 0:158dbfb66e62 737 //motor speed control(PWM)
TetsuyaKonno 0:158dbfb66e62 738 //Arguments: motor:-100 to 100
TetsuyaKonno 0:158dbfb66e62 739 //Here, 0 is stop, 100 is forward, -100 is reverse
TetsuyaKonno 0:158dbfb66e62 740 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 741 void motor( int accele_l, int accele_r )
TetsuyaKonno 0:158dbfb66e62 742 {
TetsuyaKonno 0:158dbfb66e62 743 accele_l = ( accele_l * MAX_SPEED ) / 100;
TetsuyaKonno 0:158dbfb66e62 744 accele_r = ( accele_r * MAX_SPEED ) / 100;
TetsuyaKonno 0:158dbfb66e62 745
TetsuyaKonno 0:158dbfb66e62 746 /* Left Motor Control */
TetsuyaKonno 0:158dbfb66e62 747 if( accele_l >= 0 ) {
TetsuyaKonno 0:158dbfb66e62 748 /* forward */
TetsuyaKonno 0:158dbfb66e62 749 Left_motor_signal = 0;
TetsuyaKonno 0:158dbfb66e62 750 MTU2TGRC_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * accele_l / 100;
TetsuyaKonno 0:158dbfb66e62 751 } else {
TetsuyaKonno 0:158dbfb66e62 752 /* reverse */
TetsuyaKonno 0:158dbfb66e62 753 Left_motor_signal = 1;
TetsuyaKonno 0:158dbfb66e62 754 MTU2TGRC_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * ( -accele_l ) / 100;
TetsuyaKonno 0:158dbfb66e62 755 }
TetsuyaKonno 0:158dbfb66e62 756
TetsuyaKonno 0:158dbfb66e62 757 /* Right Motor Control */
TetsuyaKonno 0:158dbfb66e62 758 if( accele_r >= 0 ) {
TetsuyaKonno 0:158dbfb66e62 759 /* forward */
TetsuyaKonno 0:158dbfb66e62 760 Right_motor_signal = 0;
TetsuyaKonno 0:158dbfb66e62 761 MTU2TGRD_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * accele_r / 100;
TetsuyaKonno 0:158dbfb66e62 762 } else {
TetsuyaKonno 0:158dbfb66e62 763 /* reverse */
TetsuyaKonno 0:158dbfb66e62 764 Right_motor_signal = 1;
TetsuyaKonno 0:158dbfb66e62 765 MTU2TGRD_4 = (long)( MOTOR_PWM_CYCLE - 1 ) * ( -accele_r ) / 100;
TetsuyaKonno 0:158dbfb66e62 766 }
TetsuyaKonno 0:158dbfb66e62 767 }
TetsuyaKonno 0:158dbfb66e62 768
TetsuyaKonno 0:158dbfb66e62 769 //handle Function
TetsuyaKonno 0:158dbfb66e62 770 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 771 void handle( int angle )
TetsuyaKonno 0:158dbfb66e62 772 {
TetsuyaKonno 0:158dbfb66e62 773 handle_buff = angle;
TetsuyaKonno 0:158dbfb66e62 774 /* When the servo move from left to right in reverse, replace "-" with "+" */
TetsuyaKonno 0:158dbfb66e62 775 MTU2TGRD_0 = SERVO_CENTER - angle * HANDLE_STEP;
TetsuyaKonno 0:158dbfb66e62 776 }
TetsuyaKonno 0:158dbfb66e62 777
TetsuyaKonno 0:158dbfb66e62 778 //diff Function
TetsuyaKonno 0:158dbfb66e62 779 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 780 int diff( int pwm )
TetsuyaKonno 0:158dbfb66e62 781 {
TetsuyaKonno 0:158dbfb66e62 782 int i, ret;
TetsuyaKonno 0:158dbfb66e62 783
TetsuyaKonno 0:158dbfb66e62 784 i = handle_buff;
TetsuyaKonno 0:158dbfb66e62 785 if( i < 0 ) i = -i;
TetsuyaKonno 0:158dbfb66e62 786 if( i > 45 ) i = 45;
TetsuyaKonno 0:158dbfb66e62 787 ret = revolution_difference[i] * pwm / 100;
TetsuyaKonno 0:158dbfb66e62 788
TetsuyaKonno 0:158dbfb66e62 789 return ret;
TetsuyaKonno 0:158dbfb66e62 790 }
TetsuyaKonno 0:158dbfb66e62 791
TetsuyaKonno 0:158dbfb66e62 792 //******************************************************************//
TetsuyaKonno 0:158dbfb66e62 793 // Image process functions
TetsuyaKonno 0:158dbfb66e62 794 //*******************************************************************/
TetsuyaKonno 0:158dbfb66e62 795 //Image Data YCbCr -> Y(320*240pix) -> Y(160*120)
TetsuyaKonno 0:158dbfb66e62 796 //frame 0 : Top field
TetsuyaKonno 0:158dbfb66e62 797 //frame 1 : Bottom field
TetsuyaKonno 0:158dbfb66e62 798 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 799 void Image_Extraction( unsigned char *buff_addr, unsigned char *Data_Y, int frame )
TetsuyaKonno 0:158dbfb66e62 800 {
TetsuyaKonno 0:158dbfb66e62 801 static int Xp, Yp, inc, Data_Y_buff;
TetsuyaKonno 0:158dbfb66e62 802 static int counter = 0;
TetsuyaKonno 0:158dbfb66e62 803
TetsuyaKonno 0:158dbfb66e62 804 // Distributed processing
TetsuyaKonno 0:158dbfb66e62 805 switch( counter++ ) {
TetsuyaKonno 0:158dbfb66e62 806 case 0:
TetsuyaKonno 0:158dbfb66e62 807 for( Yp = frame, inc = 0; Yp < 120; Yp+=2 ){
TetsuyaKonno 0:158dbfb66e62 808 for( Xp = 0; Xp < 640; Xp+=4, inc++ ){
TetsuyaKonno 0:158dbfb66e62 809 Data_Y_buff = (int)buff_addr[(Xp+0)+(640*Yp)];
TetsuyaKonno 0:158dbfb66e62 810 Data_Y_buff += (int)buff_addr[(Xp+2)+(640*Yp)];
TetsuyaKonno 0:158dbfb66e62 811 Data_Y[inc] = Data_Y_buff >> 1;
TetsuyaKonno 0:158dbfb66e62 812 }
TetsuyaKonno 0:158dbfb66e62 813 }
TetsuyaKonno 0:158dbfb66e62 814 break;
TetsuyaKonno 0:158dbfb66e62 815 case 1:
TetsuyaKonno 0:158dbfb66e62 816 for( /* None */ ; Yp < 240; Yp+=2 ){
TetsuyaKonno 0:158dbfb66e62 817 for( Xp = 0; Xp < 640; Xp+=4, inc++ ){
TetsuyaKonno 0:158dbfb66e62 818 Data_Y_buff = (int)buff_addr[(Xp+0)+(640*Yp)];
TetsuyaKonno 0:158dbfb66e62 819 Data_Y_buff += (int)buff_addr[(Xp+2)+(640*Yp)];
TetsuyaKonno 0:158dbfb66e62 820 Data_Y[inc] = Data_Y_buff >> 1;
TetsuyaKonno 0:158dbfb66e62 821 }
TetsuyaKonno 0:158dbfb66e62 822 }
TetsuyaKonno 0:158dbfb66e62 823 counter = 0;
TetsuyaKonno 0:158dbfb66e62 824 break;
TetsuyaKonno 0:158dbfb66e62 825 default:
TetsuyaKonno 0:158dbfb66e62 826 break;
TetsuyaKonno 0:158dbfb66e62 827 }
TetsuyaKonno 0:158dbfb66e62 828 }
TetsuyaKonno 0:158dbfb66e62 829
TetsuyaKonno 0:158dbfb66e62 830 //Image_Reduction Function ( Averaging processing )
TetsuyaKonno 0:158dbfb66e62 831 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 832 void Image_Reduction( unsigned char *Data_Y, int Data_W , unsigned char *Comp_Y, int Comp_M )
TetsuyaKonno 0:158dbfb66e62 833 {
TetsuyaKonno 0:158dbfb66e62 834 int Data_H, Pixel_T, Pixel_D;
TetsuyaKonno 0:158dbfb66e62 835 int x, y;
TetsuyaKonno 0:158dbfb66e62 836 static int Xp, Yp, inc;
TetsuyaKonno 0:158dbfb66e62 837 static int counter = 0;
TetsuyaKonno 0:158dbfb66e62 838
TetsuyaKonno 0:158dbfb66e62 839 Data_H = (Data_W / (double)4) * 3;
TetsuyaKonno 0:158dbfb66e62 840 Pixel_D = Comp_M * Comp_M;
TetsuyaKonno 0:158dbfb66e62 841
TetsuyaKonno 0:158dbfb66e62 842 switch( counter++ ) {
TetsuyaKonno 0:158dbfb66e62 843 case 0:
TetsuyaKonno 0:158dbfb66e62 844 for( Yp = 0, inc = 0; Yp < ( Data_H / 2); Yp+=Comp_M ){
TetsuyaKonno 0:158dbfb66e62 845 for( Xp = 0; Xp < Data_W; Xp+=Comp_M, inc++ ){
TetsuyaKonno 0:158dbfb66e62 846 Pixel_T = 0;
TetsuyaKonno 0:158dbfb66e62 847 for( y = 0; y < Comp_M; y++ ){
TetsuyaKonno 0:158dbfb66e62 848 for( x = 0; x < Comp_M; x++ ){
TetsuyaKonno 0:158dbfb66e62 849 Pixel_T += Data_Y[( Xp + x ) + (( Yp + y ) * Data_W )];
TetsuyaKonno 0:158dbfb66e62 850 }
TetsuyaKonno 0:158dbfb66e62 851 }
TetsuyaKonno 0:158dbfb66e62 852 Comp_Y[inc] = Pixel_T / Pixel_D;
TetsuyaKonno 0:158dbfb66e62 853 }
TetsuyaKonno 0:158dbfb66e62 854 }
TetsuyaKonno 0:158dbfb66e62 855 break;
TetsuyaKonno 0:158dbfb66e62 856 case 1:
TetsuyaKonno 0:158dbfb66e62 857 for( /* None */ ; Yp < Data_H ; Yp+=Comp_M ){
TetsuyaKonno 0:158dbfb66e62 858 for( Xp = 0; Xp < Data_W; Xp+=Comp_M, inc++ ){
TetsuyaKonno 0:158dbfb66e62 859 Pixel_T = 0;
TetsuyaKonno 0:158dbfb66e62 860 for( y = 0; y < Comp_M; y++ ){
TetsuyaKonno 0:158dbfb66e62 861 for( x = 0; x < Comp_M; x++ ){
TetsuyaKonno 0:158dbfb66e62 862 Pixel_T += Data_Y[( Xp + x ) + (( Yp + y ) * Data_W )];
TetsuyaKonno 0:158dbfb66e62 863 }
TetsuyaKonno 0:158dbfb66e62 864 }
TetsuyaKonno 0:158dbfb66e62 865 Comp_Y[inc] = Pixel_T / Pixel_D;
TetsuyaKonno 0:158dbfb66e62 866 }
TetsuyaKonno 0:158dbfb66e62 867 }
TetsuyaKonno 0:158dbfb66e62 868 counter = 0;
TetsuyaKonno 0:158dbfb66e62 869 break;
TetsuyaKonno 0:158dbfb66e62 870 default:
TetsuyaKonno 0:158dbfb66e62 871 break;
TetsuyaKonno 0:158dbfb66e62 872 }
TetsuyaKonno 0:158dbfb66e62 873 }
TetsuyaKonno 0:158dbfb66e62 874
TetsuyaKonno 0:158dbfb66e62 875 // Binarization_process Function
TetsuyaKonno 0:158dbfb66e62 876 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 877 void Binarization_process( unsigned char *Comp_Y, unsigned char *Binary, long items, int threshold )
TetsuyaKonno 0:158dbfb66e62 878 {
TetsuyaKonno 0:158dbfb66e62 879 int i;
TetsuyaKonno 0:158dbfb66e62 880
TetsuyaKonno 0:158dbfb66e62 881 for( i = 0; i < items; i++ ) {
TetsuyaKonno 0:158dbfb66e62 882 if( Comp_Y[i] >= threshold ) Binary[i] = 1;
TetsuyaKonno 0:158dbfb66e62 883 else Binary[i] = 0;
TetsuyaKonno 0:158dbfb66e62 884 }
TetsuyaKonno 0:158dbfb66e62 885 }
TetsuyaKonno 0:158dbfb66e62 886
TetsuyaKonno 0:158dbfb66e62 887 //******************************************************************//
TetsuyaKonno 0:158dbfb66e62 888 // digital sensor functions
TetsuyaKonno 0:158dbfb66e62 889 //*******************************************************************/
TetsuyaKonno 0:158dbfb66e62 890 //CenterLine_Corrective Function ( image size 20*15pix )
TetsuyaKonno 0:158dbfb66e62 891 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 892 int CenterLine_Corrective( unsigned char *Binary )
TetsuyaKonno 0:158dbfb66e62 893 {
TetsuyaKonno 0:158dbfb66e62 894 #define L 0
TetsuyaKonno 0:158dbfb66e62 895 #define R 1
TetsuyaKonno 0:158dbfb66e62 896
TetsuyaKonno 0:158dbfb66e62 897 int iRet, offset_X, offset_Y;
TetsuyaKonno 0:158dbfb66e62 898 int Xpix, X;
TetsuyaKonno 0:158dbfb66e62 899 int Ypix;
TetsuyaKonno 0:158dbfb66e62 900 int Pixel_diff[2];
TetsuyaKonno 0:158dbfb66e62 901 int Error_cnt;
TetsuyaKonno 0:158dbfb66e62 902 int value;
TetsuyaKonno 0:158dbfb66e62 903
TetsuyaKonno 0:158dbfb66e62 904 /* Center of image */
TetsuyaKonno 0:158dbfb66e62 905 offset_X = 6;
TetsuyaKonno 0:158dbfb66e62 906 offset_Y = 12;
TetsuyaKonno 0:158dbfb66e62 907
TetsuyaKonno 0:158dbfb66e62 908 /* corrective of center line */
TetsuyaKonno 0:158dbfb66e62 909 for( Ypix = 0, Error_cnt = 0; Ypix < (offset_Y - 4); Ypix++ ) {
TetsuyaKonno 0:158dbfb66e62 910 for( value = 0; value < 2; value++ ) {
TetsuyaKonno 0:158dbfb66e62 911 for( Xpix = offset_X; Xpix < (offset_X + 8); Xpix++ ) {
TetsuyaKonno 0:158dbfb66e62 912 /* Lift side */
TetsuyaKonno 0:158dbfb66e62 913 Pixel_diff[L] = 0;
TetsuyaKonno 0:158dbfb66e62 914 if( Binary[ ( ( offset_Y - Ypix ) * 20 ) + Xpix ] >= 1 ) {
TetsuyaKonno 0:158dbfb66e62 915 for( X = Xpix; X > (Xpix - 4); X-- ) {
TetsuyaKonno 0:158dbfb66e62 916 if( Binary[ ( ( offset_Y - Ypix ) * 20 ) + X ] >= 1 ) {
TetsuyaKonno 0:158dbfb66e62 917 Pixel_diff[L]++;
TetsuyaKonno 0:158dbfb66e62 918 } else {
TetsuyaKonno 0:158dbfb66e62 919 break;
TetsuyaKonno 0:158dbfb66e62 920 }
TetsuyaKonno 0:158dbfb66e62 921 }
TetsuyaKonno 0:158dbfb66e62 922 } else {
TetsuyaKonno 0:158dbfb66e62 923 Pixel_diff[L] = -1;
TetsuyaKonno 0:158dbfb66e62 924 }
TetsuyaKonno 0:158dbfb66e62 925 /* Right side */
TetsuyaKonno 0:158dbfb66e62 926 Pixel_diff[R] = 0;
TetsuyaKonno 0:158dbfb66e62 927 if( Binary[ ( ( offset_Y - Ypix ) * 20 ) + (Xpix + 1) ] >= 1 ) {
TetsuyaKonno 0:158dbfb66e62 928 for( X = (Xpix + 1); X < ((Xpix + 1) + 4); X++ ) {
TetsuyaKonno 0:158dbfb66e62 929 if( Binary[ ( ( offset_Y - Ypix ) * 20 ) + X ] >= 1 ) {
TetsuyaKonno 0:158dbfb66e62 930 Pixel_diff[R]++;
TetsuyaKonno 0:158dbfb66e62 931 } else {
TetsuyaKonno 0:158dbfb66e62 932 break;
TetsuyaKonno 0:158dbfb66e62 933 }
TetsuyaKonno 0:158dbfb66e62 934 }
TetsuyaKonno 0:158dbfb66e62 935 } else {
TetsuyaKonno 0:158dbfb66e62 936 Pixel_diff[R] = 1;
TetsuyaKonno 0:158dbfb66e62 937 }
TetsuyaKonno 0:158dbfb66e62 938 /* check */
TetsuyaKonno 0:158dbfb66e62 939 iRet = Pixel_diff[L] - Pixel_diff[R];
TetsuyaKonno 0:158dbfb66e62 940 if( value >= iRet && iRet >= -value ) {
TetsuyaKonno 0:158dbfb66e62 941 break;
TetsuyaKonno 0:158dbfb66e62 942 }
TetsuyaKonno 0:158dbfb66e62 943 }
TetsuyaKonno 0:158dbfb66e62 944 if( value >= iRet && iRet >= -value ) {
TetsuyaKonno 0:158dbfb66e62 945 /* X coordinate */
TetsuyaKonno 0:158dbfb66e62 946 Sensor_X[Ypix][2] = Xpix;
TetsuyaKonno 0:158dbfb66e62 947 Sensor_X[Ypix][3] = Xpix + 1;
TetsuyaKonno 0:158dbfb66e62 948 break;
TetsuyaKonno 0:158dbfb66e62 949 } else {
TetsuyaKonno 0:158dbfb66e62 950 Sensor_X[Ypix][2] = Sensor_X[Ypix][3] = -1;
TetsuyaKonno 0:158dbfb66e62 951 Error_cnt++;
TetsuyaKonno 0:158dbfb66e62 952 }
TetsuyaKonno 0:158dbfb66e62 953 }
TetsuyaKonno 0:158dbfb66e62 954 /* Left side sensor */
TetsuyaKonno 0:158dbfb66e62 955 Sensor_X[Ypix][1] = Sensor_X[Ypix][2] - ( Pixel_diff[L] );
TetsuyaKonno 0:158dbfb66e62 956 Sensor_X[Ypix][0] = Sensor_X[Ypix][1] - ( Pixel_diff[L] + 1 );//( Sensor_X[Ypix][2] - Sensor_X[Ypix][1] );
TetsuyaKonno 0:158dbfb66e62 957 /* Right side sensor */
TetsuyaKonno 0:158dbfb66e62 958 Sensor_X[Ypix][4] = Sensor_X[Ypix][3] + ( Pixel_diff[R] );
TetsuyaKonno 0:158dbfb66e62 959 Sensor_X[Ypix][5] = Sensor_X[Ypix][4] + ( Pixel_diff[R] + 1 );//( Sensor_X[Ypix][4] - Sensor_X[Ypix][3] );
TetsuyaKonno 0:158dbfb66e62 960 }
TetsuyaKonno 0:158dbfb66e62 961 return Error_cnt;
TetsuyaKonno 0:158dbfb66e62 962 }
TetsuyaKonno 0:158dbfb66e62 963
TetsuyaKonno 0:158dbfb66e62 964 //digital_sensor_process Function
TetsuyaKonno 0:158dbfb66e62 965 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 966 void digital_sensor_process( unsigned char *Binary )
TetsuyaKonno 0:158dbfb66e62 967 {
TetsuyaKonno 0:158dbfb66e62 968 int Ypix;
TetsuyaKonno 0:158dbfb66e62 969 int offset_Y;
TetsuyaKonno 0:158dbfb66e62 970 unsigned char sensor, data;
TetsuyaKonno 0:158dbfb66e62 971
TetsuyaKonno 0:158dbfb66e62 972 offset_Y = 12;
TetsuyaKonno 0:158dbfb66e62 973 sensor = 0;
TetsuyaKonno 0:158dbfb66e62 974
TetsuyaKonno 0:158dbfb66e62 975 data = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][2] ] & 0x01;
TetsuyaKonno 0:158dbfb66e62 976 data |= Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][3] ] & 0x01;
TetsuyaKonno 0:158dbfb66e62 977 sensor |= (data << 4) & 0x10;
TetsuyaKonno 0:158dbfb66e62 978 data = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][0] ] & 0x01;
TetsuyaKonno 0:158dbfb66e62 979 sensor |= (data << 3) & 0x08;
TetsuyaKonno 0:158dbfb66e62 980 data = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][1] ] & 0x01;
TetsuyaKonno 0:158dbfb66e62 981 sensor |= (data << 2) & 0x04;
TetsuyaKonno 0:158dbfb66e62 982 data = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][4] ] & 0x01;
TetsuyaKonno 0:158dbfb66e62 983 sensor |= (data << 1) & 0x02;
TetsuyaKonno 0:158dbfb66e62 984 data = Binary[ ( (offset_Y - Ypix) * 20 ) + Sensor_X[Ypix][5] ] & 0x01;
TetsuyaKonno 0:158dbfb66e62 985 sensor |= (data << 0) & 0x01;
TetsuyaKonno 0:158dbfb66e62 986 sensor &= 0x1f;
TetsuyaKonno 0:158dbfb66e62 987 sensor_value = sensor;
TetsuyaKonno 0:158dbfb66e62 988 }
TetsuyaKonno 0:158dbfb66e62 989
TetsuyaKonno 0:158dbfb66e62 990 //digital_sensor Function
TetsuyaKonno 0:158dbfb66e62 991 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 992 unsigned char digital_sensor( void )
TetsuyaKonno 0:158dbfb66e62 993 {
TetsuyaKonno 0:158dbfb66e62 994 return sensor_value;
TetsuyaKonno 0:158dbfb66e62 995 }
TetsuyaKonno 0:158dbfb66e62 996
TetsuyaKonno 0:158dbfb66e62 997 //******************************************************************//
TetsuyaKonno 0:158dbfb66e62 998 // Mark detect functions
TetsuyaKonno 0:158dbfb66e62 999 //*******************************************************************/
TetsuyaKonno 0:158dbfb66e62 1000 // Extract_Image
TetsuyaKonno 0:158dbfb66e62 1001 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 1002 void Image_part_Extraction( unsigned char *Binary, int Width, int Xpix, int Ypix, unsigned char *Data_B, int x_size, int y_size )
TetsuyaKonno 0:158dbfb66e62 1003 {
TetsuyaKonno 0:158dbfb66e62 1004 int x, y;
TetsuyaKonno 0:158dbfb66e62 1005 for( y = 0; y < y_size; y++ ) {
TetsuyaKonno 0:158dbfb66e62 1006 for( x = 0; x < x_size; x++ ) {
TetsuyaKonno 0:158dbfb66e62 1007 Data_B[ x + ( y * x_size ) ] = Binary[ (Xpix + x) + ( (Ypix + y) * Width ) ];
TetsuyaKonno 0:158dbfb66e62 1008 }
TetsuyaKonno 0:158dbfb66e62 1009 }
TetsuyaKonno 0:158dbfb66e62 1010 }
TetsuyaKonno 0:158dbfb66e62 1011
TetsuyaKonno 0:158dbfb66e62 1012 // Standard deviation
TetsuyaKonno 0:158dbfb66e62 1013 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 1014 double Standard_Deviation( unsigned char *data, double *Devi, int items )
TetsuyaKonno 0:158dbfb66e62 1015 {
TetsuyaKonno 0:158dbfb66e62 1016 int i;
TetsuyaKonno 0:158dbfb66e62 1017 double iRet_A, iRet_C, iRet_D;
TetsuyaKonno 0:158dbfb66e62 1018
TetsuyaKonno 0:158dbfb66e62 1019 /* A 合計値　平均化 */
TetsuyaKonno 0:158dbfb66e62 1020 iRet_A = 0;
TetsuyaKonno 0:158dbfb66e62 1021 for( i = 0; i < items; i++ ) {
TetsuyaKonno 0:158dbfb66e62 1022 iRet_A += data[i];
TetsuyaKonno 0:158dbfb66e62 1023 }
TetsuyaKonno 0:158dbfb66e62 1024 iRet_A /= items;
TetsuyaKonno 0:158dbfb66e62 1025
TetsuyaKonno 0:158dbfb66e62 1026 /* B 偏差値 */
TetsuyaKonno 0:158dbfb66e62 1027 for( i = 0; i < items; i++ ) {
TetsuyaKonno 0:158dbfb66e62 1028 Devi[i] = data[i] - iRet_A;
TetsuyaKonno 0:158dbfb66e62 1029 }
TetsuyaKonno 0:158dbfb66e62 1030
TetsuyaKonno 0:158dbfb66e62 1031 /* C 分散 */
TetsuyaKonno 0:158dbfb66e62 1032 iRet_C = 0;
TetsuyaKonno 0:158dbfb66e62 1033 for( i = 0; i < items; i++ ) {
TetsuyaKonno 0:158dbfb66e62 1034 iRet_C += ( Devi[i] * Devi[i] );
TetsuyaKonno 0:158dbfb66e62 1035 }
TetsuyaKonno 0:158dbfb66e62 1036 iRet_C /= items;
TetsuyaKonno 0:158dbfb66e62 1037
TetsuyaKonno 0:158dbfb66e62 1038 /* D 標準偏差 */
TetsuyaKonno 0:158dbfb66e62 1039 iRet_D = sqrt( iRet_C );
TetsuyaKonno 0:158dbfb66e62 1040
TetsuyaKonno 0:158dbfb66e62 1041 return iRet_D;
TetsuyaKonno 0:158dbfb66e62 1042 }
TetsuyaKonno 0:158dbfb66e62 1043
TetsuyaKonno 0:158dbfb66e62 1044 // Covariance
TetsuyaKonno 0:158dbfb66e62 1045 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 1046 double Covariance( double *Devi_A, double *Devi_B, int items )
TetsuyaKonno 0:158dbfb66e62 1047 {
TetsuyaKonno 0:158dbfb66e62 1048 int i;
TetsuyaKonno 0:158dbfb66e62 1049 double iRet, iRet_buff;
TetsuyaKonno 0:158dbfb66e62 1050
TetsuyaKonno 0:158dbfb66e62 1051 iRet = 0;
TetsuyaKonno 0:158dbfb66e62 1052 for( i = 0; i < items; i++ ) {
TetsuyaKonno 0:158dbfb66e62 1053 iRet_buff = Devi_A[i] * Devi_B[i];
TetsuyaKonno 0:158dbfb66e62 1054 iRet += iRet_buff;
TetsuyaKonno 0:158dbfb66e62 1055 }
TetsuyaKonno 0:158dbfb66e62 1056 iRet /= items;
TetsuyaKonno 0:158dbfb66e62 1057
TetsuyaKonno 0:158dbfb66e62 1058 return iRet;
TetsuyaKonno 0:158dbfb66e62 1059 }
TetsuyaKonno 0:158dbfb66e62 1060
TetsuyaKonno 0:158dbfb66e62 1061 // Judgement_ImageMatching
TetsuyaKonno 0:158dbfb66e62 1062 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 1063 int Judgement_ImageMatching( double covari, double SDevi_A, double SDevi_B )
TetsuyaKonno 0:158dbfb66e62 1064 {
TetsuyaKonno 0:158dbfb66e62 1065 int iRet;
TetsuyaKonno 0:158dbfb66e62 1066
TetsuyaKonno 0:158dbfb66e62 1067 iRet = ( covari * 100 ) / ( SDevi_A * SDevi_B );
TetsuyaKonno 0:158dbfb66e62 1068
TetsuyaKonno 0:158dbfb66e62 1069 return iRet;
TetsuyaKonno 0:158dbfb66e62 1070 }
TetsuyaKonno 0:158dbfb66e62 1071
TetsuyaKonno 0:158dbfb66e62 1072 // MarkDetect_process_T
TetsuyaKonno 0:158dbfb66e62 1073 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 1074 void MarkDetect_process_T( void )
TetsuyaKonno 0:158dbfb66e62 1075 {
TetsuyaKonno 0:158dbfb66e62 1076 int x, y;
TetsuyaKonno 0:158dbfb66e62 1077
TetsuyaKonno 0:158dbfb66e62 1078 retJudgeIM_Max[0] = 0;
TetsuyaKonno 0:158dbfb66e62 1079 for( y = 0; y <= 12; y++ ) {
TetsuyaKonno 0:158dbfb66e62 1080 for( x = 0; x <= 15; x++ ) {
TetsuyaKonno 0:158dbfb66e62 1081 Image_part_Extraction( ImageBinary, 20, x, y, NowImageBinary, 5, 3 );
TetsuyaKonno 0:158dbfb66e62 1082 retDevi = Standard_Deviation( NowImageBinary, NowDevi, 15 );
TetsuyaKonno 0:158dbfb66e62 1083 retCovari = Covariance( TempDevi_Triangle, NowDevi, 15 );
TetsuyaKonno 0:158dbfb66e62 1084 retJudgeIM = 0;
TetsuyaKonno 0:158dbfb66e62 1085 retJudgeIM = Judgement_ImageMatching( retCovari, retDevi_Triangle, retDevi );
TetsuyaKonno 0:158dbfb66e62 1086 if( 100 >= retJudgeIM && retJudgeIM > retJudgeIM_Max[0] ) {
TetsuyaKonno 0:158dbfb66e62 1087 Xt = x;
TetsuyaKonno 0:158dbfb66e62 1088 Yt = y;
TetsuyaKonno 0:158dbfb66e62 1089 retJudgeIM_Max[0] = retJudgeIM;
TetsuyaKonno 0:158dbfb66e62 1090 }
TetsuyaKonno 0:158dbfb66e62 1091 }
TetsuyaKonno 0:158dbfb66e62 1092 }
TetsuyaKonno 0:158dbfb66e62 1093 }
TetsuyaKonno 0:158dbfb66e62 1094
TetsuyaKonno 0:158dbfb66e62 1095 // MarkCheck Triangle detection
TetsuyaKonno 0:158dbfb66e62 1096 // Return values: 0: no triangle mark, 1: Triangle mark
TetsuyaKonno 0:158dbfb66e62 1097 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 1098 int MarkCheck_Triangle( int percentage )
TetsuyaKonno 0:158dbfb66e62 1099 {
TetsuyaKonno 0:158dbfb66e62 1100 int ret;
TetsuyaKonno 0:158dbfb66e62 1101
TetsuyaKonno 0:158dbfb66e62 1102 ret = 0;
TetsuyaKonno 0:158dbfb66e62 1103 if( retJudgeIM_Max[0] >= percentage ) {
TetsuyaKonno 0:158dbfb66e62 1104 ret = 1;
TetsuyaKonno 0:158dbfb66e62 1105 }
TetsuyaKonno 0:158dbfb66e62 1106
TetsuyaKonno 0:158dbfb66e62 1107 return ret;
TetsuyaKonno 0:158dbfb66e62 1108 }
TetsuyaKonno 0:158dbfb66e62 1109
TetsuyaKonno 0:158dbfb66e62 1110 //******************************************************************//
TetsuyaKonno 0:158dbfb66e62 1111 // Debug functions
TetsuyaKonno 0:158dbfb66e62 1112 //*******************************************************************/
TetsuyaKonno 0:158dbfb66e62 1113 //Image Data Output( for the Excel )
TetsuyaKonno 0:158dbfb66e62 1114 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 1115 void ImageData_Serial_Out( unsigned char *Data_Y, int Width )
TetsuyaKonno 0:158dbfb66e62 1116 {
TetsuyaKonno 0:158dbfb66e62 1117 int Xp, Yp, inc, Height;
TetsuyaKonno 0:158dbfb66e62 1118
TetsuyaKonno 0:158dbfb66e62 1119 Height = (Width / (double)4) * 3;
TetsuyaKonno 0:158dbfb66e62 1120 for( Yp = 0, inc = 0; Yp < Height; Yp++ ) {
TetsuyaKonno 0:158dbfb66e62 1121 for( Xp = 0; Xp < Width; Xp++, inc++ ) {
TetsuyaKonno 0:158dbfb66e62 1122 pc.printf( "%d,", Data_Y[ inc ] );
TetsuyaKonno 0:158dbfb66e62 1123 }
TetsuyaKonno 0:158dbfb66e62 1124 pc.printf("\n\r");
TetsuyaKonno 0:158dbfb66e62 1125 }
TetsuyaKonno 0:158dbfb66e62 1126 }
TetsuyaKonno 0:158dbfb66e62 1127
TetsuyaKonno 0:158dbfb66e62 1128 //Image Data Output2( for TeraTerm )
TetsuyaKonno 0:158dbfb66e62 1129 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 1130 void ImageData_Serial_Out2( unsigned char *Data_Y, int Width )
TetsuyaKonno 0:158dbfb66e62 1131 {
TetsuyaKonno 0:158dbfb66e62 1132 int Xp, Yp, Height;
TetsuyaKonno 0:158dbfb66e62 1133
TetsuyaKonno 0:158dbfb66e62 1134 Height = (Width / (double)4) * 3;
TetsuyaKonno 0:158dbfb66e62 1135 for( Yp = 0; Yp < Height; Yp++ ) {
TetsuyaKonno 0:158dbfb66e62 1136 for( Xp = 0; Xp < Width; Xp++ ) {
TetsuyaKonno 0:158dbfb66e62 1137 pc.printf( "%d ", Data_Y[Xp + (Yp * Width)] );
TetsuyaKonno 0:158dbfb66e62 1138 }
TetsuyaKonno 0:158dbfb66e62 1139 pc.printf( "\n\r" );
TetsuyaKonno 0:158dbfb66e62 1140 }
TetsuyaKonno 0:158dbfb66e62 1141
TetsuyaKonno 0:158dbfb66e62 1142 //Add display
TetsuyaKonno 0:158dbfb66e62 1143 pc.printf( "\n\r" );
TetsuyaKonno 0:158dbfb66e62 1144 pc.printf( "sensor_inp = 0x%02x\n\r", digital_sensor() );
TetsuyaKonno 0:158dbfb66e62 1145 pc.printf( "T = %3d%% %01d X=%2d Y=%2d\n\r", retJudgeIM_Max[0], MarkCheck_Triangle( 90 ), Xt, Yt );
TetsuyaKonno 0:158dbfb66e62 1146 pc.printf( "\n\r" );
TetsuyaKonno 0:158dbfb66e62 1147 Height += 4;
TetsuyaKonno 0:158dbfb66e62 1148
TetsuyaKonno 0:158dbfb66e62 1149 pc.printf( "\033[%dA" , Height );
TetsuyaKonno 0:158dbfb66e62 1150 }
TetsuyaKonno 0:158dbfb66e62 1151
TetsuyaKonno 0:158dbfb66e62 1152 //------------------------------------------------------------------//
TetsuyaKonno 0:158dbfb66e62 1153 // End of file
TetsuyaKonno 0:158dbfb66e62 1154 //------------------------------------------------------------------//