Micon Car Rally / Mbed 2 deprecated TraceMark_Program

This program is Trace and Mark detection Program by Image Processing.

Dependencies:   GR-PEACH_video mbed

main.cpp@0:55e74a8600c6, 2016-09-02 (annotated)

Committer:
TetsuyaKonno
Date:
Fri Sep 02 07:34:13 2016 +0000
Revision:
0:55e74a8600c6
First program

Who changed what in which revision?

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